From 182b71217dbac43dd5dbd5ff1f436dfc72fd25bd Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Sun, 1 Feb 2026 23:06:35 +0100 Subject: [PATCH 01/11] feat(arcor2_ur): extend ros_worker for collision primitives and add basic tests --- src/python/arcor2_ur/scripts/ros_worker.py | 36 ++++++--- .../arcor2_ur/tests/test_box_in_path.py | 53 ++++++++++++++ .../arcor2_ur/tests/test_cylinder_in_path.py | 53 ++++++++++++++ .../arcor2_ur/tests/test_sphere_in_path.py | 73 +++++++++++++++++++ 4 files changed, 204 insertions(+), 11 deletions(-) create mode 100644 src/python/arcor2_ur/tests/test_box_in_path.py create mode 100644 src/python/arcor2_ur/tests/test_cylinder_in_path.py create mode 100644 src/python/arcor2_ur/tests/test_sphere_in_path.py diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index 3e80e981..bf5d2c29 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -300,26 +300,40 @@ def joint_states_cb(self, msg: JointState) -> None: def apply_collision_objects( scene, collision_objects: dict[str, CollisionObjectTuple], base_pose: Pose, base_link: str ) -> None: - scene.remove_all_collision_objects() + scene.remove_all_collision_objects() # just to be sure that scene is clean for update for obj_id, obj in collision_objects.items(): - if not isinstance(obj.model, object_type.Box): - continue - collision_object = CollisionObject() collision_object.header.frame_id = base_link collision_object.id = obj_id - box_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) + obj_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) + prim = SolidPrimitive() - box = SolidPrimitive() - box.type = SolidPrimitive.BOX - box.dimensions = (obj.model.size_x, obj.model.size_y, obj.model.size_z) + if isinstance(obj.model, object_type.Box): + prim.type = SolidPrimitive.BOX + prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] - collision_object.primitives.append(box) - collision_object.primitive_poses.append(box_pose) - collision_object.operation = CollisionObject.ADD + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(obj_pose) + + elif isinstance(obj.model, object_type.Sphere): + prim.type = SolidPrimitive.SPHERE + prim.dimensions = [obj.model.radius] + + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(obj_pose) + elif isinstance(obj.model, object_type.Cylinder): + prim.type = SolidPrimitive.CYLINDER + prim.dimensions = [obj.model.height, obj.model.radius] + + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(obj_pose) + + # elif isinstance(obj.model, object_type.Mesh): + + collision_object.operation = CollisionObject.ADD scene.apply_collision_object(collision_object) scene.current_state.update() diff --git a/src/python/arcor2_ur/tests/test_box_in_path.py b/src/python/arcor2_ur/tests/test_box_in_path.py new file mode 100644 index 00000000..8a5ef730 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_box_in_path.py @@ -0,0 +1,53 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_box_in_path(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box1", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box2", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_cylinder_in_path.py b/src/python/arcor2_ur/tests/test_cylinder_in_path.py new file mode 100644 index 00000000..6c830da9 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_cylinder_in_path.py @@ -0,0 +1,53 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_cylinder_in_path(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_sphere_in_path.py b/src/python/arcor2_ur/tests/test_sphere_in_path.py new file mode 100644 index 00000000..12f8df6a --- /dev/null +++ b/src/python/arcor2_ur/tests/test_sphere_in_path.py @@ -0,0 +1,73 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_sphere_in_path(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere2", 0.1) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere3", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere4", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere5", 0.1) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere6", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() From 01ae28456e41444ac3017c01a50ab39b6d1b3d17 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Thu, 5 Mar 2026 22:19:15 +0100 Subject: [PATCH 02/11] feat(arcor2_ur): graspable obj representation, basic functionality tests --- build-support/install_ur_dependencies.sh | 12 + src/python/arcor2_object_types/abstract.py | 121 ++++- src/python/arcor2_scene/scripts/scene.py | 395 +++++++++++++++ src/python/arcor2_scene_data/scene_service.py | 63 +++ src/python/arcor2_ur/scripts/ros_worker.py | 145 ++++-- src/python/arcor2_ur/scripts/ur.py | 450 +++++++++++++++++- src/python/arcor2_ur/tests/test_box_up.py | 63 +++ .../arcor2_ur/tests/test_cylinder_up.py | 63 +++ .../tests/test_grasapble_collizion_object.py | 62 +++ src/python/arcor2_ur/tests/test_sphere_up.py | 63 +++ 10 files changed, 1401 insertions(+), 36 deletions(-) create mode 100644 src/python/arcor2_ur/tests/test_box_up.py create mode 100644 src/python/arcor2_ur/tests/test_cylinder_up.py create mode 100644 src/python/arcor2_ur/tests/test_grasapble_collizion_object.py create mode 100644 src/python/arcor2_ur/tests/test_sphere_up.py diff --git a/build-support/install_ur_dependencies.sh b/build-support/install_ur_dependencies.sh index c1a82933..d96e6e12 100755 --- a/build-support/install_ur_dependencies.sh +++ b/build-support/install_ur_dependencies.sh @@ -1,7 +1,19 @@ #!/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 \ +<<<<<<< HEAD ros-jazzy-ros-base=0.11.0-1noble.20260412.071950 \ ros-jazzy-ur=3.7.0-1noble.20260412.082316 \ ros-jazzy-moveit-py=2.12.4-1noble.20260412.073026 +======= + ros-jazzy-ros-base=0.11.0-1noble.20251108.003726 \ + ros-jazzy-ur=3.6.0-1noble.20251114.095610 \ + ros-jazzy-moveit-py=2.12.3-1noble.20251108.011222 +>>>>>>> 32612146 (feat(arcor2_ur): graspable obj representation, basic functionality tests) diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index 01a64c2c..5c431940 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/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, Parameter, Pose, SceneObject from arcor2.data.object_type import Models, PrimitiveModels from arcor2.data.robot import RobotType from arcor2.docstring import parse_docstring @@ -183,6 +185,123 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None: set_enabled.__action__ = ActionMetadata() # type: ignore +class GraspableState(str, Enum): + WORLD = "WORLD" + ATTACHED = "ATTACHED" + RESERVED = "RESERVED" + LOST = "LOST" + + +class GraspableSource(str, Enum): + CAMERA = "camera" + FIXED = "fixed" + OTHER = "other" + + +def _utc_now_iso() -> str: + return datetime.now(timezone.utc).isoformat() + + +class GraspableObject(GenericWithPose): + 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() + + self.collision_model.id = self.id + _scene_service().upsert_graspable( + self.collision_model, + pose, + state=self._state.value, + source=self._source.value, + stamp=self._stamp, + ) + + def set_pose(self, pose: Pose) -> None: + super().set_pose(pose) + if self._enabled: + _scene_service().upsert_graspable( + self.collision_model, + pose, + state=self._state.value, + source=self._source.value, + stamp=self._stamp, + ) + + @property + def enabled(self) -> bool: + svc = _scene_service() + assert (self.id in svc.graspable_ids()) == self._enabled + return self._enabled + + @enabled.setter + def enabled(self, enabled: bool) -> None: + svc = _scene_service() + + if not self._enabled and enabled: + assert self.id not in svc.graspable_ids() + svc.upsert_graspable( + self.collision_model, + self.pose, + state=self._state.value, + source=self._source.value, + stamp=self._stamp, + ) + + if self._enabled and not enabled: + svc.delete_graspable_id(self.id) + + self._enabled = enabled + + @property + def state(self) -> GraspableState: + return self._state + + def set_state(self, state: GraspableState, *, an: None | str = None) -> None: + self._state = state + # re-upsert so server knows about new state + if self._enabled: + _scene_service().upsert_graspable( + self.collision_model, + self.pose, + state=self._state.value, + source=self._source.value, + stamp=self._stamp, + ) + + set_state.__action__ = ActionMetadata() # type: ignore + + @property + def source(self) -> GraspableSource: + return self._source + + @property + def stamp(self) -> str: + return self._stamp + + def set_enabled(self, state: bool, *, an: None | str = None) -> None: + self.enabled = state + + set_enabled.__action__ = ActionMetadata() # type: ignore + + class VirtualCollisionObject(CollisionObject): """Should be used to represent obstacles or another 'dumb' objects at the workplace.""" diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index 08ac5977..defba6d9 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -30,10 +30,27 @@ class CollisionObject(NamedTuple): pose: common.Pose +class GraspableObject(NamedTuple): + model: object_type.Models + pose: common.Pose + state: str + source: str + stamp: str + + collision_objects: dict[str, CollisionObject] = {} +graspable_objects: dict[str, GraspableObject] = {} started: bool = False inflation = 0.01 + +def _graspable_meta_from_request() -> tuple[str, str, str]: + state = request.args.get("state", default="WORLD") + source = request.args.get("source", default="other") + stamp = request.args.get("stamp", default="") + return state, source, stamp + + delay_mean = env.get_float("ARCOR2_SCENE_DELAY_MEAN", 0) delay_sigma = env.get_float("ARCOR2_SCENE_DELAY_SIGMA", 0) @@ -516,6 +533,7 @@ def put_stop() -> RespT: delay() started = False collision_objects.clear() + graspable_objects.clear() return Response(status=200) @@ -545,6 +563,383 @@ def get_started() -> RespT: return jsonify(started) +@app.route("/graspables/box", methods=["PUT"]) +def put_graspable_box() -> RespT: + """Add or update graspable box. + --- + put: + tags: + - Graspables + description: Add or update graspable box. + parameters: + - name: boxId + in: query + description: unique graspable box ID + required: true + schema: + type: string + - name: sizeX + in: query + schema: + type: number + format: float + - name: sizeY + in: query + schema: + type: number + format: float + - name: sizeZ + in: query + schema: + type: number + format: float + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 200: + description: Ok + content: + application/json: + schema: + type: string + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = request.args.to_dict() + box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"])) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + graspable_objects[box.id] = GraspableObject( + model=box, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + return jsonify("ok"), 200 + + +@app.route("/graspables/sphere", methods=["PUT"]) +def put_graspable_sphere() -> RespT: + """Add or update graspable sphere. + --- + put: + tags: + - Graspables + description: Add or update graspable sphere. + parameters: + - name: sphereId + in: query + description: unique graspable sphere ID + required: true + schema: + type: string + - name: radius + in: query + schema: + type: number + format: float + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 200: + description: Ok + content: + application/json: + schema: + type: string + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = humps.decamelize(request.args.to_dict()) + sphere = object_type.Sphere(args["sphere_id"], float(args["radius"])) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + graspable_objects[sphere.id] = GraspableObject( + model=sphere, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + return jsonify("ok"), 200 + + +@app.route("/graspables/cylinder", methods=["PUT"]) +def put_graspable_cylinder() -> RespT: + """Add or update graspable cylinder. + --- + put: + tags: + - Graspables + description: Add or update graspable cylinder. + parameters: + - name: cylinderId + in: query + description: unique graspable cylinder ID + required: true + schema: + type: string + - name: radius + in: query + schema: + type: number + format: float + - name: height + in: query + schema: + type: number + format: float + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 200: + description: Ok + content: + application/json: + schema: + type: string + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = humps.decamelize(request.args.to_dict()) + cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"])) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + graspable_objects[cylinder.id] = GraspableObject( + model=cylinder, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + return jsonify("ok"), 200 + + +@app.route("/graspables/mesh", methods=["PUT"]) +def put_graspable_mesh() -> RespT: + """Add or update graspable mesh. + --- + put: + tags: + - Graspables + description: Add or update graspable mesh. + parameters: + - name: meshId + in: query + description: unique graspable mesh ID + required: true + schema: + type: string + - name: meshFileId + in: query + schema: + type: string + - name: meshScaleX + in: query + schema: + type: number + format: float + default: 1.0 + - name: meshScaleY + in: query + schema: + type: number + format: float + default: 1.0 + - name: meshScaleZ + in: query + schema: + type: number + format: float + default: 1.0 + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 200: + description: Ok + content: + application/json: + schema: + type: string + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = humps.decamelize(request.args.to_dict()) + mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + graspable_objects[mesh.id] = GraspableObject( + model=mesh, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + return jsonify("ok"), 200 + + +@app.route("/graspables/", methods=["DELETE"]) +def delete_graspable(id: str) -> RespT: + """Deletes graspable object. + --- + delete: + tags: + - Graspables + summary: Deletes graspable object. + parameters: + - name: id + in: path + description: unique graspable ID + required: true + schema: + type: string + responses: + 200: + description: Ok + 500: + description: "Error types: **General**, **NotFound**." + content: + application/json: + schema: + $ref: WebApiError + """ + + try: + del graspable_objects[id] + except KeyError: + raise NotFound("Graspable not found") + + return Response(status=200) + + +@app.route("/graspables", methods=["GET"]) +def get_graspables() -> RespT: + """Gets graspable ids. + --- + get: + tags: + - Graspables + summary: Gets graspable ids. + responses: + 200: + description: Success + content: + application/json: + schema: + type: array + items: + type: string + 500: + description: "Error types: **General**." + content: + application/json: + schema: + $ref: WebApiError + """ + + return jsonify(list(graspable_objects.keys())) + + +@app.route("/graspables/", methods=["GET"]) +def get_graspable(id: str) -> RespT: + """Gets graspable detail. + --- + get: + tags: + - Graspables + summary: Gets graspable detail. + parameters: + - name: id + in: path + description: unique graspable ID + required: true + schema: + type: string + responses: + 200: + description: Success + 500: + description: "Error types: **General**, **NotFound**." + content: + application/json: + schema: + $ref: WebApiError + """ + try: + entry = graspable_objects[id] + except KeyError: + raise NotFound("Graspable not found") + + return jsonify( + { + "id": id, + "modelType": entry.model.type().value.lower(), + "model": entry.model.to_dict(), + "pose": entry.pose.to_dict(), + "state": entry.state, + "source": entry.source, + "stamp": entry.stamp, + } + ) + + def main() -> None: global inflation diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py index 18f0aa35..6840ceee 100644 --- a/src/python/arcor2_scene_data/scene_service.py +++ b/src/python/arcor2_scene_data/scene_service.py @@ -85,6 +85,64 @@ def collision_ids() -> set[str]: return set(rest.call(rest.Method.GET, f"{URL}/collisions", list_return_type=str)) +@handle(SceneServiceException, logger, message="Failed to add or update the graspable model.") +def upsert_graspable( + model: Models, + pose: Pose, + mesh_parameters: None | MeshParameters = None, + *, + state: str | None = None, + source: str | None = None, + stamp: str | None = None, +) -> None: + params = model.to_dict() + model_type = model.type().value.lower() + + params[f"{model_type}Id"] = model.id + del params["id"] + + if state is not None: + params["state"] = state + if source is not None: + params["source"] = source + if stamp is not None: + params["stamp"] = stamp + + if model.type() == Model3dType.MESH: + params["meshFileId"] = params.pop("asset_id") + if mesh_parameters: + params.update(mesh_parameters.to_dict()) + + rest.call(rest.Method.PUT, f"{URL}/graspables/{model_type}", body=pose, params=params) + + +@handle(SceneServiceException, logger, message="Failed to list graspables.") +def graspable_ids() -> set[str]: + return set(rest.call(rest.Method.GET, f"{URL}/graspables", list_return_type=str)) + + +@handle(SceneServiceException, logger, message="Failed to get graspable states.") +def graspable_states() -> dict[str, str]: + states: dict[str, str] = {} + for gid in graspable_ids(): + payload = rest.call(rest.Method.GET, f"{URL}/graspables/{gid}", return_type=dict) + state = payload.get("state") + if not isinstance(state, str): + raise SceneServiceException(f"Graspable {gid} has invalid or missing 'state'.") + states[gid] = state + return states + + +@handle(SceneServiceException, logger, message="Failed to delete the graspable.") +def delete_graspable_id(graspable_id: str) -> None: + rest.call(rest.Method.DELETE, f"{URL}/graspables/{graspable_id}") + + +def delete_all_graspable() -> None: + for cid in collision_ids(): + delete_collision_id(cid) + + @handle(SceneServiceException, logger, message="Failed to focus the object.") def focus(mfa: MeshFocusAction) -> Pose: return rest.call(rest.Method.PUT, f"{URL}/utils/focus", body=mfa, return_type=Pose) @@ -167,4 +225,9 @@ def world_pose(transform_id: str) -> Pose: upsert_transform.__name__, local_pose.__name__, world_pose.__name__, + upsert_graspable.__name__, + delete_graspable_id.__name__, + graspable_ids.__name__, + delete_all_graspable.__name__, + graspable_states.__name__, ] diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index bf5d2c29..7aaadd50 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -1,20 +1,24 @@ import importlib import multiprocessing +import struct +import sys import threading import time from multiprocessing.connection import Connection from typing import Any, Callable, NamedTuple, cast import rclpy # pants: no-infer-dep -from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep from geometry_msgs.msg import PoseStamped # pants: no-infer-dep +from geometry_msgs.msg import Point +from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep from moveit_msgs.msg import CollisionObject # pants: no-infer-dep from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep from sensor_msgs.msg import JointState # pants: no-infer-dep -from shape_msgs.msg import SolidPrimitive # pants: no-infer-dep +from shape_msgs.msg import Mesh as RosMesh # pants: no-infer-dep +from shape_msgs.msg import MeshTriangle, SolidPrimitive from std_msgs.msg import Bool, String # pants: no-infer-dep from std_srvs.srv import Trigger # pants: no-infer-dep from tf2_ros.buffer import Buffer # pants: no-infer-dep @@ -45,6 +49,14 @@ class CollisionObjectTuple(NamedTuple): pose: common.Pose +class GraspableObjectTuple(NamedTuple): + model: object_type.Models + pose: common.Pose + state: str + source: str + stamp: str + + def pose_to_ros_pose(ps: Pose) -> RosPose: rp = RosPose() rp.position.x = ps.position.x @@ -297,45 +309,57 @@ def joint_states_cb(self, msg: JointState) -> None: self._joint_state_handler(list(msg.name), list(msg.position)) -def apply_collision_objects( - scene, collision_objects: dict[str, CollisionObjectTuple], base_pose: Pose, base_link: str -) -> None: - scene.remove_all_collision_objects() # just to be sure that scene is clean for update +def create_collision_object(obj, obj_id, base_link: str, base_pose: Pose) -> CollisionObject: + collision_object = CollisionObject() + collision_object.header.frame_id = base_link + collision_object.id = obj_id - for obj_id, obj in collision_objects.items(): - collision_object = CollisionObject() - collision_object.header.frame_id = base_link - collision_object.id = obj_id + obj_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) + prim = SolidPrimitive() - obj_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) - prim = SolidPrimitive() + if isinstance(obj.model, object_type.Box): + prim.type = SolidPrimitive.BOX + prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] - if isinstance(obj.model, object_type.Box): - prim.type = SolidPrimitive.BOX - prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] + elif isinstance(obj.model, object_type.Sphere): + prim.type = SolidPrimitive.SPHERE + prim.dimensions = [obj.model.radius] - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(obj_pose) + elif isinstance(obj.model, object_type.Cylinder): + prim.type = SolidPrimitive.CYLINDER + prim.dimensions = [obj.model.height, obj.model.radius] - elif isinstance(obj.model, object_type.Sphere): - prim.type = SolidPrimitive.SPHERE - prim.dimensions = [obj.model.radius] + # elif isinstance(obj.model, object_type.Mesh): - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(obj_pose) + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(obj_pose) - elif isinstance(obj.model, object_type.Cylinder): - prim.type = SolidPrimitive.CYLINDER - prim.dimensions = [obj.model.height, obj.model.radius] + collision_object.operation = CollisionObject.ADD + return collision_object - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(obj_pose) - # elif isinstance(obj.model, object_type.Mesh): +def apply_collision_objects( + scene, + collision_objects: dict[str, CollisionObjectTuple], + graspable_objects: dict[str, GraspableObjectTuple], + base_pose: Pose, + base_link: str, +) -> None: + scene.remove_all_collision_objects() # just to be sure that scene is clean for update + + for obj_id, obj in collision_objects.items(): + collision_object = create_collision_object(obj, obj_id, base_link, base_pose) collision_object.operation = CollisionObject.ADD scene.apply_collision_object(collision_object) + for obj_id, obj in graspable_objects.items(): + if obj.state == "WORLD": # graspable objects which are not moving + + collision_object = create_collision_object(obj, obj_id, base_link, base_pose) + collision_object.operation = CollisionObject.ADD + scene.apply_collision_object(collision_object) + scene.current_state.update() @@ -346,6 +370,7 @@ def __init__( base_link: str, tool_link: str, collision_objects: dict[str, CollisionObjectTuple], + graspable_objects: dict[str, GraspableObjectTuple], interact_with_dashboard: bool, robot_ip: str, vgc10_port: int, @@ -356,7 +381,10 @@ def __init__( self.base_pose = base_pose self.base_link = base_link self.tool_link = tool_link + self.collision_objects = collision_objects.copy() + self.graspable_objects = graspable_objects.copy() + self.joints: list[Joint] = [] self.tool: VGC10 | None = None self.freedrive_mode = False @@ -418,7 +446,13 @@ def __init__( moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + apply_collision_objects( + scene, + collision_objects=self.collision_objects, + graspable_objects=self.graspable_objects, + base_pose=self.base_pose, + base_link=self.base_link, + ) def _update_joints(self, names: list[str], positions: list[float]) -> None: self.joints = [Joint(name, position) for name, position in zip(names, positions)] @@ -612,7 +646,13 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) moveitpy, ur_manipulator = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if safe: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + apply_collision_objects( + scene, + collision_objects=self.collision_objects, + graspable_objects=self.graspable_objects, + base_pose=self.base_pose, + base_link=self.base_link, + ) else: scene.remove_all_collision_objects() @@ -649,7 +689,13 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]: moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if ikr.avoid_collisions: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + apply_collision_objects( + scene, + collision_objects=self.collision_objects, + graspable_objects=self.graspable_objects, + base_pose=self.base_pose, + base_link=self.base_link, + ) else: scene.remove_all_collision_objects() @@ -672,7 +718,13 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]: raise UrGeneral("State is in collision.") except UrGeneral: if not ikr.avoid_collisions: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + apply_collision_objects( + scene, + collision_objects=self.collision_objects, + graspable_objects=self.graspable_objects, + base_pose=self.base_pose, + base_link=self.base_link, + ) raise if len(scene.current_state.joint_positions) != len(self.joints): @@ -693,7 +745,26 @@ def update_collisions(self, collision_objects: dict[str, CollisionObjectTuple]) self.collision_objects = collision_objects.copy() moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + apply_collision_objects( + scene, + collision_objects=self.collision_objects, + graspable_objects=self.graspable_objects, + base_pose=self.base_pose, + base_link=self.base_link, + ) + return {} + + def update_graspables(self, graspable_objects: dict[str, GraspableObjectTuple]) -> dict: + self.graspable_objects = graspable_objects.copy() + moveitpy, _ = self._get_moveit() + with moveitpy.get_planning_scene_monitor().read_write() as scene: + apply_collision_objects( + scene, + collision_objects=self.collision_objects, + graspable_objects=self.graspable_objects, + base_pose=self.base_pose, + base_link=self.base_link, + ) return {} def suck(self, vacuum: int) -> dict: @@ -728,6 +799,7 @@ def ros_worker_main( base_link: str, tool_link: str, collision_objects: dict[str, CollisionObjectTuple], + graspable_objects: dict[str, GraspableObjectTuple], interact_with_dashboard: bool, robot_ip: str, vgc10_port: int, @@ -743,6 +815,7 @@ def ros_worker_main( base_link, tool_link, collision_objects, + graspable_objects, interact_with_dashboard, robot_ip, vgc10_port, @@ -751,7 +824,7 @@ def ros_worker_main( debug, ) conn.send({"status": "ok"}) - except Exception as exc: # pragma: no cover - best effort to report start failure + except Exception as exc: # pragma: no cover logger.exception("Failed to start ROS worker.") try: conn.send({"status": "error", "message": str(exc)}) @@ -796,6 +869,8 @@ def ros_worker_main( result = runtime.get_freedrive_mode() elif op == "update_collisions": result = runtime.update_collisions(kwargs["collision_objects"]) + elif op == "update_graspables": + result = runtime.update_graspables(kwargs["graspable_objects"]) elif op == "suck": result = runtime.suck(kwargs["vacuum"]) elif op == "release": @@ -826,6 +901,7 @@ def __init__( self, pose: Pose, collision_objects: dict[str, CollisionObjectTuple], + graspable_objects: dict[str, GraspableObjectTuple], base_link: str, tool_link: str, planning_group_name: str, @@ -845,6 +921,7 @@ def __init__( base_link, tool_link, collision_objects, + graspable_objects, interact_with_dashboard, robot_ip, vgc10_port, diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index 24fe4913..19b2e053 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -20,7 +20,7 @@ from arcor2_ur import get_data, version from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum -from arcor2_ur.scripts.ros_worker import CollisionObjectTuple, RosWorkerClient +from arcor2_ur.scripts.ros_worker import CollisionObjectTuple, GraspableObjectTuple, RosWorkerClient from arcor2_web.flask import RespT, create_app, run_app logger = get_logger(__name__) @@ -48,9 +48,17 @@ class Globs: debug = False state: ServiceState | None = None collision_objects: dict[str, CollisionObjectTuple] = field(default_factory=dict) + graspable_objects: dict[str, GraspableObjectTuple] = field(default_factory=dict) scene_started = False # flag for "Scene service" +def _graspable_meta_from_request() -> tuple[str, str, str]: + state = request.args.get("state", default="WORLD") + source = request.args.get("source", default="other") + stamp = request.args.get("stamp", default="") + return state, source, stamp + + globs: Globs = Globs() app = create_app(__name__) @@ -129,6 +137,7 @@ def put_start() -> RespT: worker = RosWorkerClient( pose, globs.collision_objects, + globs.graspable_objects, BASE_LINK, TOOL_LINK, PLANNING_GROUP_NAME, @@ -802,6 +811,445 @@ def put_release() -> RespT: return Response(status=204) +@app.route("/graspables/box", methods=["PUT"]) +def put_graspable_box() -> RespT: + """Add or update graspable box. + --- + put: + tags: + - Graspables + description: Add or update graspable box. + parameters: + - name: boxId + in: query + description: unique graspable box ID + required: true + schema: + type: string + - name: sizeX + in: query + schema: + type: number + format: float + - name: sizeY + in: query + schema: + type: number + format: float + - name: sizeZ + in: query + schema: + type: number + format: float + - name: state + in: query + schema: + type: string + default: WORLD + - name: source + in: query + schema: + type: string + default: other + - name: stamp + in: query + schema: + type: string + default: "" + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **UrGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = request.args.to_dict() + box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"])) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + globs.graspable_objects[box.id] = GraspableObjectTuple( + model=box, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + # collision-like update logic, but for graspables + if started(): + assert globs.state + globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) + + return Response(status=204) + + +@app.route("/graspables/sphere", methods=["PUT"]) +def put_graspable_sphere() -> RespT: + """Add or update graspable sphere. + --- + put: + tags: + - Graspables + description: Add or update graspable sphere. + parameters: + - name: sphereId + in: query + description: unique graspable sphere ID + required: true + schema: + type: string + - name: radius + in: query + schema: + type: number + format: float + - name: state + in: query + schema: + type: string + default: WORLD + - name: source + in: query + schema: + type: string + default: other + - name: stamp + in: query + schema: + type: string + default: "" + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **UrGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = humps.decamelize(request.args.to_dict()) + sphere = object_type.Sphere(args["sphere_id"], float(args["radius"])) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + globs.graspable_objects[sphere.id] = GraspableObjectTuple( + model=sphere, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + if started(): + assert globs.state + globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) + + return Response(status=204) + + +@app.route("/graspables/cylinder", methods=["PUT"]) +def put_graspable_cylinder() -> RespT: + """Add or update graspable cylinder. + --- + put: + tags: + - Graspables + description: Add or update graspable cylinder. + parameters: + - name: cylinderId + in: query + description: unique graspable cylinder ID + required: true + schema: + type: string + - name: radius + in: query + schema: + type: number + format: float + - name: height + in: query + schema: + type: number + format: float + - name: state + in: query + schema: + type: string + default: WORLD + - name: source + in: query + schema: + type: string + default: other + - name: stamp + in: query + schema: + type: string + default: "" + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **UrGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = humps.decamelize(request.args.to_dict()) + cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"])) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + globs.graspable_objects[cylinder.id] = GraspableObjectTuple( + model=cylinder, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + if started(): + assert globs.state + globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) + + return Response(status=204) + + +@app.route("/graspables/mesh", methods=["PUT"]) +def put_graspable_mesh() -> RespT: + """Add or update graspable mesh. + --- + put: + tags: + - Graspables + description: Add or update graspable mesh. + parameters: + - name: meshId + in: query + description: unique graspable mesh ID + required: true + schema: + type: string + - name: meshFileId + in: query + schema: + type: string + - name: meshScaleX + in: query + schema: + type: number + format: float + default: 1.0 + - name: meshScaleY + in: query + schema: + type: number + format: float + default: 1.0 + - name: meshScaleZ + in: query + schema: + type: number + format: float + default: 1.0 + - name: state + in: query + schema: + type: string + default: WORLD + - name: source + in: query + schema: + type: string + default: other + - name: stamp + in: query + schema: + type: string + default: "" + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **UrGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + state, source, stamp = _graspable_meta_from_request() + + args = humps.decamelize(request.args.to_dict()) + mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) + pose = common.Pose.from_dict(humps.decamelize(request.json)) + + globs.graspable_objects[mesh.id] = GraspableObjectTuple( + model=mesh, + pose=pose, + state=state, + source=source, + stamp=stamp, + ) + + if started(): + assert globs.state + globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) + + return Response(status=204) + + +@app.route("/graspables/", methods=["DELETE"]) +def delete_graspable(id: str) -> RespT: + """Deletes graspable object. + --- + delete: + tags: + - Graspables + summary: Deletes graspable object. + parameters: + - name: id + in: path + description: unique graspable ID + required: true + schema: + type: string + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **NotFound**." + content: + application/json: + schema: + $ref: WebApiError + """ + try: + del globs.graspable_objects[id] + except KeyError: + raise NotFound("Graspable not found") + + if started(): + assert globs.state + globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) + + return Response(status=204) + + +@app.route("/graspables", methods=["GET"]) +def get_graspables() -> RespT: + """Gets graspable ids. + --- + get: + tags: + - Graspables + summary: Gets graspable ids. + responses: + 200: + description: Success + content: + application/json: + schema: + type: array + items: + type: string + 500: + description: "Error types: **General**." + content: + application/json: + schema: + $ref: WebApiError + """ + return jsonify(list(globs.graspable_objects.keys())) + + +@app.route("/graspables/", methods=["GET"]) +def get_graspable(id: str) -> RespT: + """Gets graspable detail. + --- + get: + tags: + - Graspables + summary: Gets graspable detail. + parameters: + - name: id + in: path + description: unique graspable ID + required: true + schema: + type: string + responses: + 200: + description: Success + 500: + description: "Error types: **General**, **NotFound**." + content: + application/json: + schema: + $ref: WebApiError + """ + try: + entry = globs.graspable_objects[id] + except KeyError: + raise NotFound("Graspable not found") + + model = entry.model + pose = entry.pose + + return jsonify( + { + "id": id, + "modelType": model.type().value.lower(), + "model": model.to_dict(), + "pose": pose.to_dict(), + "state": entry.state, + "source": entry.source, + "stamp": entry.stamp, + } + ) + + def main() -> None: parser = argparse.ArgumentParser(description=SERVICE_NAME) parser.add_argument("-s", "--swagger", action="store_true", default=False) diff --git a/src/python/arcor2_ur/tests/test_box_up.py b/src/python/arcor2_ur/tests/test_box_up.py new file mode 100644 index 00000000..8b983dd3 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_box_up.py @@ -0,0 +1,63 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_box_up(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box2", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box3", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box4", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_cylinder_up.py b/src/python/arcor2_ur/tests/test_cylinder_up.py new file mode 100644 index 00000000..5c957b83 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_cylinder_up.py @@ -0,0 +1,63 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_cylinder_up(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl3", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl4", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_grasapble_collizion_object.py b/src/python/arcor2_ur/tests/test_grasapble_collizion_object.py new file mode 100644 index 00000000..723dea20 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_grasapble_collizion_object.py @@ -0,0 +1,62 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_sphere_up(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.1, 0.95) + scene_service.upsert_graspable( + cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), state="WORLD", source="test" + ) + assert cyl.id in scene_service.graspable_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl3", 0.1, 0.95) + scene_service.upsert_graspable( + cyl, Pose(Position(0.0, 0.0, 0.0), Orientation(0, 0, 0, 1)), state="ATTACHED", source="test" + ) + assert cyl.id in scene_service.graspable_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_sphere_up.py b/src/python/arcor2_ur/tests/test_sphere_up.py new file mode 100644 index 00000000..25f8649b --- /dev/null +++ b/src/python/arcor2_ur/tests/test_sphere_up.py @@ -0,0 +1,63 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def _dist(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +@pytest.mark.timeout(220) +def test_sphere_up(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere2", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere3", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere4", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert _dist(reached, goal_pose) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() From d66180677b34669247aaf0705f9f92196b16915c Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Mon, 23 Mar 2026 21:38:18 +0100 Subject: [PATCH 03/11] feat(graspable): update grasp metadata, refactor tests and add move_object_to_pose skeleton --- src/python/arcor2/data/common.py | 6 + src/python/arcor2_object_types/abstract.py | 134 ++-- src/python/arcor2_scene/scripts/scene.py | 436 +----------- src/python/arcor2_scene_data/scene_service.py | 171 +++-- src/python/arcor2_ur/object_types/ur5e.py | 17 +- src/python/arcor2_ur/scripts/ros_worker.py | 262 ++++---- src/python/arcor2_ur/scripts/ur.py | 635 +++++------------- .../arcor2_ur/tests/test_box_in_path.py | 53 -- src/python/arcor2_ur/tests/test_box_up.py | 63 -- .../arcor2_ur/tests/test_cylinder_in_path.py | 53 -- .../arcor2_ur/tests/test_cylinder_up.py | 63 -- .../tests/test_grasapble_collizion_object.py | 62 -- .../tests/test_graspable_collision_object.py | 59 ++ .../arcor2_ur/tests/test_overhead_obstacle.py | 148 ++++ .../arcor2_ur/tests/test_reserve_object.py | 49 ++ .../arcor2_ur/tests/test_side_obstacle.py | 138 ++++ .../arcor2_ur/tests/test_sphere_in_path.py | 73 -- src/python/arcor2_ur/tests/test_sphere_up.py | 63 -- 18 files changed, 916 insertions(+), 1569 deletions(-) delete mode 100644 src/python/arcor2_ur/tests/test_box_in_path.py delete mode 100644 src/python/arcor2_ur/tests/test_box_up.py delete mode 100644 src/python/arcor2_ur/tests/test_cylinder_in_path.py delete mode 100644 src/python/arcor2_ur/tests/test_cylinder_up.py delete mode 100644 src/python/arcor2_ur/tests/test_grasapble_collizion_object.py create mode 100644 src/python/arcor2_ur/tests/test_graspable_collision_object.py create mode 100644 src/python/arcor2_ur/tests/test_overhead_obstacle.py create mode 100644 src/python/arcor2_ur/tests/test_reserve_object.py create mode 100644 src/python/arcor2_ur/tests/test_side_obstacle.py delete mode 100644 src/python/arcor2_ur/tests/test_sphere_in_path.py delete mode 100644 src/python/arcor2_ur/tests/test_sphere_up.py diff --git a/src/python/arcor2/data/common.py b/src/python/arcor2/data/common.py index 0d12f093..7708d5d4 100644 --- a/src/python/arcor2/data/common.py +++ b/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, diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index 5c431940..7abf8bb1 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -10,7 +10,7 @@ from arcor2 import CancelDict, DynamicParamDict from arcor2.data.camera import CameraParameters -from arcor2.data.common import ActionMetadata, Joint, Parameter, 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 @@ -185,24 +185,54 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None: set_enabled.__action__ = ActionMetadata() # type: ignore -class GraspableState(str, Enum): +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. + + ATTACHED + Attached to the robot end-effector. + + LOST + Object not observable. Location unknown. + """ + WORLD = "WORLD" - ATTACHED = "ATTACHED" RESERVED = "RESERVED" + ATTACHED = "ATTACHED" LOST = "LOST" -class GraspableSource(str, Enum): - CAMERA = "camera" - FIXED = "fixed" - OTHER = "other" +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__( @@ -220,86 +250,98 @@ def __init__( 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, - pose, - state=self._state.value, - source=self._source.value, + 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: - _scene_service().upsert_graspable( - self.collision_model, - pose, - state=self._state.value, - source=self._source.value, - stamp=self._stamp, - ) + 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.graspable_ids()) == self._enabled + assert (self.id in svc.collision_ids()) == self._enabled return self._enabled @enabled.setter def enabled(self, enabled: bool) -> None: - svc = _scene_service() - if not self._enabled and enabled: - assert self.id not in svc.graspable_ids() - svc.upsert_graspable( - self.collision_model, - self.pose, - state=self._state.value, - source=self._source.value, - stamp=self._stamp, - ) - + svc = _scene_service() + assert self.id not in svc.collision_ids() + self._upsert_graspable() if self._enabled and not enabled: - svc.delete_graspable_id(self.id) - + _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 - def set_state(self, state: GraspableState, *, an: None | str = None) -> None: + @state.setter + def state(self, state: GraspableState) -> None: self._state = state - # re-upsert so server knows about new state - if self._enabled: - _scene_service().upsert_graspable( - self.collision_model, - self.pose, - state=self._state.value, - source=self._source.value, - stamp=self._stamp, - ) + self._stamp = _utc_now_iso() - set_state.__action__ = ActionMetadata() # type: ignore + 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_enabled(self, state: bool, *, an: None | str = None) -> None: - self.enabled = state + def set_state(self, state: GraspableState, *, an: None | str = None) -> None: + self.state = state - set_enabled.__action__ = ActionMetadata() # type: ignore + 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): diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index defba6d9..b0a116a9 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -5,7 +5,8 @@ import math import random import time -from typing import NamedTuple +from dataclasses import dataclass, field +from typing import Any import humps import numpy as np @@ -18,39 +19,18 @@ 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.scripts.ros_worker import CollisionSceneObject +from arcor2_ur.scripts.ur import 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 - - -class GraspableObject(NamedTuple): - model: object_type.Models - pose: common.Pose - state: str - source: str - stamp: str - - -collision_objects: dict[str, CollisionObject] = {} -graspable_objects: dict[str, GraspableObject] = {} +collision_objects: dict[str, CollisionSceneObject] = {} started: bool = False inflation = 0.01 - -def _graspable_meta_from_request() -> tuple[str, str, str]: - state = request.args.get("state", default="WORLD") - source = request.args.get("source", default="other") - stamp = request.args.get("stamp", default="") - return state, source, stamp - - delay_mean = env.get_float("ARCOR2_SCENE_DELAY_MEAN", 0) delay_sigma = env.get_float("ARCOR2_SCENE_DELAY_SIGMA", 0) @@ -98,7 +78,7 @@ def put_box() -> RespT: content: application/json: schema: - $ref: Pose + type: object responses: 200: description: Ok @@ -114,12 +94,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 @@ -148,7 +127,7 @@ def put_sphere() -> RespT: content: application/json: schema: - $ref: Pose + type: object responses: 200: description: Ok @@ -164,12 +143,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 @@ -202,7 +180,7 @@ def put_cylinder() -> RespT: content: application/json: schema: - $ref: Pose + type: object responses: 200: description: Ok @@ -218,12 +196,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 @@ -268,7 +245,7 @@ def put_mesh() -> RespT: content: application/json: schema: - $ref: Pose + type: object responses: 200: description: Ok @@ -284,12 +261,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 @@ -533,7 +509,6 @@ def put_stop() -> RespT: delay() started = False collision_objects.clear() - graspable_objects.clear() return Response(status=200) @@ -563,383 +538,6 @@ def get_started() -> RespT: return jsonify(started) -@app.route("/graspables/box", methods=["PUT"]) -def put_graspable_box() -> RespT: - """Add or update graspable box. - --- - put: - tags: - - Graspables - description: Add or update graspable box. - parameters: - - name: boxId - in: query - description: unique graspable box ID - required: true - schema: - type: string - - name: sizeX - in: query - schema: - type: number - format: float - - name: sizeY - in: query - schema: - type: number - format: float - - name: sizeZ - in: query - schema: - type: number - format: float - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 200: - description: Ok - content: - application/json: - schema: - type: string - 500: - description: "Error types: **General**, **SceneGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = request.args.to_dict() - box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"])) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - graspable_objects[box.id] = GraspableObject( - model=box, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - return jsonify("ok"), 200 - - -@app.route("/graspables/sphere", methods=["PUT"]) -def put_graspable_sphere() -> RespT: - """Add or update graspable sphere. - --- - put: - tags: - - Graspables - description: Add or update graspable sphere. - parameters: - - name: sphereId - in: query - description: unique graspable sphere ID - required: true - schema: - type: string - - name: radius - in: query - schema: - type: number - format: float - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 200: - description: Ok - content: - application/json: - schema: - type: string - 500: - description: "Error types: **General**, **SceneGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = humps.decamelize(request.args.to_dict()) - sphere = object_type.Sphere(args["sphere_id"], float(args["radius"])) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - graspable_objects[sphere.id] = GraspableObject( - model=sphere, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - return jsonify("ok"), 200 - - -@app.route("/graspables/cylinder", methods=["PUT"]) -def put_graspable_cylinder() -> RespT: - """Add or update graspable cylinder. - --- - put: - tags: - - Graspables - description: Add or update graspable cylinder. - parameters: - - name: cylinderId - in: query - description: unique graspable cylinder ID - required: true - schema: - type: string - - name: radius - in: query - schema: - type: number - format: float - - name: height - in: query - schema: - type: number - format: float - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 200: - description: Ok - content: - application/json: - schema: - type: string - 500: - description: "Error types: **General**, **SceneGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = humps.decamelize(request.args.to_dict()) - cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"])) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - graspable_objects[cylinder.id] = GraspableObject( - model=cylinder, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - return jsonify("ok"), 200 - - -@app.route("/graspables/mesh", methods=["PUT"]) -def put_graspable_mesh() -> RespT: - """Add or update graspable mesh. - --- - put: - tags: - - Graspables - description: Add or update graspable mesh. - parameters: - - name: meshId - in: query - description: unique graspable mesh ID - required: true - schema: - type: string - - name: meshFileId - in: query - schema: - type: string - - name: meshScaleX - in: query - schema: - type: number - format: float - default: 1.0 - - name: meshScaleY - in: query - schema: - type: number - format: float - default: 1.0 - - name: meshScaleZ - in: query - schema: - type: number - format: float - default: 1.0 - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 200: - description: Ok - content: - application/json: - schema: - type: string - 500: - description: "Error types: **General**, **SceneGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = humps.decamelize(request.args.to_dict()) - mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - graspable_objects[mesh.id] = GraspableObject( - model=mesh, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - return jsonify("ok"), 200 - - -@app.route("/graspables/", methods=["DELETE"]) -def delete_graspable(id: str) -> RespT: - """Deletes graspable object. - --- - delete: - tags: - - Graspables - summary: Deletes graspable object. - parameters: - - name: id - in: path - description: unique graspable ID - required: true - schema: - type: string - responses: - 200: - description: Ok - 500: - description: "Error types: **General**, **NotFound**." - content: - application/json: - schema: - $ref: WebApiError - """ - - try: - del graspable_objects[id] - except KeyError: - raise NotFound("Graspable not found") - - return Response(status=200) - - -@app.route("/graspables", methods=["GET"]) -def get_graspables() -> RespT: - """Gets graspable ids. - --- - get: - tags: - - Graspables - summary: Gets graspable ids. - responses: - 200: - description: Success - content: - application/json: - schema: - type: array - items: - type: string - 500: - description: "Error types: **General**." - content: - application/json: - schema: - $ref: WebApiError - """ - - return jsonify(list(graspable_objects.keys())) - - -@app.route("/graspables/", methods=["GET"]) -def get_graspable(id: str) -> RespT: - """Gets graspable detail. - --- - get: - tags: - - Graspables - summary: Gets graspable detail. - parameters: - - name: id - in: path - description: unique graspable ID - required: true - schema: - type: string - responses: - 200: - description: Success - 500: - description: "Error types: **General**, **NotFound**." - content: - application/json: - schema: - $ref: WebApiError - """ - try: - entry = graspable_objects[id] - except KeyError: - raise NotFound("Graspable not found") - - return jsonify( - { - "id": id, - "modelType": entry.model.type().value.lower(), - "model": entry.model.to_dict(), - "pose": entry.pose.to_dict(), - "state": entry.state, - "source": entry.source, - "stamp": entry.stamp, - } - ) - - def main() -> None: global inflation diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py index 6840ceee..bcc5b8c4 100644 --- a/src/python/arcor2_scene_data/scene_service.py +++ b/src/python/arcor2_scene_data/scene_service.py @@ -1,15 +1,18 @@ import os import time -from dataclasses import dataclass +from dataclasses import dataclass, field +from datetime import datetime, timezone +from typing import Any from dataclasses_jsonschema import JsonSchemaMixin -from arcor2.data.common import Pose +from arcor2.data.common import Pose, Position from arcor2.data.object_type import Model3dType, Models from arcor2.data.scene import LineCheck, LineCheckResult, MeshFocusAction from arcor2.exceptions import Arcor2Exception from arcor2.exceptions.helpers import handle from arcor2.logging import get_logger +from arcor2_object_types.abstract import GraspableSource, GraspableState from arcor2_web import rest URL = os.getenv("ARCOR2_SCENE_SERVICE_URL", "http://0.0.0.0:5013") @@ -22,6 +25,18 @@ class SceneServiceException(Arcor2Exception): pass +@dataclass +class CollisionBody(JsonSchemaMixin): + pose: Pose + metadata: dict[str, Any] = field(default_factory=dict) + + +@dataclass +class ReserveGraspable(JsonSchemaMixin): + position: Position + radius: float + + @dataclass class MeshParameters(JsonSchemaMixin): mesh_scale_x: float = 1.0 @@ -30,6 +45,23 @@ class MeshParameters(JsonSchemaMixin): transform_id: str = "world" +def _utc_now_iso() -> str: + return datetime.now(timezone.utc).isoformat() + + +def _graspable_metadata( + state: GraspableState = GraspableState.WORLD, + source: GraspableSource = GraspableSource.OTHER, + stamp: str | None = None, +) -> dict[str, Any]: + return { + "object_type": "graspable", + "state": state.value, + "source": source.value, + "stamp": stamp or _utc_now_iso(), + } + + def wait_for(timeout: float = 10.0) -> None: start_time = time.monotonic() while time.monotonic() < start_time + timeout: @@ -42,8 +74,28 @@ def wait_for(timeout: float = 10.0) -> None: raise SceneServiceException("Failed to contact Scene service.") +def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: + params = model.to_dict() + model_type = model.type().value.lower() + + params[f"{model_type}Id"] = model.id + del params["id"] + + if model.type() == Model3dType.MESH: + params["meshFileId"] = params.pop("asset_id") + + if mesh_parameters: + params.update(mesh_parameters.to_dict()) + + return params, model_type + + @handle(SceneServiceException, logger, message="Failed to add or update the collision model.") -def upsert_collision(model: Models, pose: Pose, mesh_parameters: None | MeshParameters = None) -> None: +def upsert_collision( + model: Models, + pose: Pose, + mesh_parameters: None | MeshParameters = None, +) -> None: """Adds arbitrary collision model to the collision scene. :param model: Box, Sphere, Cylinder, Mesh @@ -60,87 +112,67 @@ def upsert_collision(model: Models, pose: Pose, mesh_parameters: None | MeshPara >>> scene_service.upsert_collision(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1))) """ - params = model.to_dict() - model_type = model.type().value.lower() - - params[f"{model_type}Id"] = model.id - del params["id"] - - if model.type() == Model3dType.MESH: - params["meshFileId"] = params.pop("asset_id") - - if mesh_parameters: - params.update(mesh_parameters.to_dict()) - - rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=pose, params=params) + params, model_type = _collision_params(model, mesh_parameters) + body = CollisionBody(pose, {}) + rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params) -@handle(SceneServiceException, logger, message="Failed to delete the collision.") -def delete_collision_id(collision_id: str) -> None: - rest.call(rest.Method.DELETE, f"{URL}/collisions/{collision_id}") - - -@handle(SceneServiceException, logger, message="Failed to list collisions.") -def collision_ids() -> set[str]: - return set(rest.call(rest.Method.GET, f"{URL}/collisions", list_return_type=str)) - - -@handle(SceneServiceException, logger, message="Failed to add or update the graspable model.") +@handle(SceneServiceException, logger, message="Failed to add or update the graspable object.") def upsert_graspable( model: Models, pose: Pose, - mesh_parameters: None | MeshParameters = None, - *, - state: str | None = None, - source: str | None = None, + state: GraspableState, + source: GraspableSource = GraspableSource.OTHER, stamp: str | None = None, + mesh_parameters: None | MeshParameters = None, ) -> None: - params = model.to_dict() - model_type = model.type().value.lower() - - params[f"{model_type}Id"] = model.id - del params["id"] + """Adds arbitrary graspable object to the collision scene. - if state is not None: - params["state"] = state - if source is not None: - params["source"] = source - if stamp is not None: - params["stamp"] = stamp + Internally, graspable objects are transferred using collision API enriched + by graspable metadata. - if model.type() == Model3dType.MESH: - params["meshFileId"] = params.pop("asset_id") - if mesh_parameters: - params.update(mesh_parameters.to_dict()) + :param model: Box, Sphere, Cylinder, Mesh + :param pose: Pose of the graspable object. + :param state: Logical state of the graspable object. + :param source: Source of the graspable pose information. + :param stamp: Optional timestamp for graspable metadata. + :param mesh_parameters: Some additional parameters might be specified for mesh collision model. + :return: - rest.call(rest.Method.PUT, f"{URL}/graspables/{model_type}", body=pose, params=params) + Example usage: + >>> from arcor2_scene_data import scene_service + >>> from arcor2.data.object_type import Box + >>> from arcor2.data.common import Pose, Position, Orientation + >>> from arcor2_object_types.abstract import GraspableState + >>> box = Box("UniqueBoxId", 0.1, 0.1, 0.1) + >>> scene_service.upsert_graspable( + ... box, + ... Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1)), + ... state=GraspableState.WORLD, + ... ) + """ -@handle(SceneServiceException, logger, message="Failed to list graspables.") -def graspable_ids() -> set[str]: - return set(rest.call(rest.Method.GET, f"{URL}/graspables", list_return_type=str)) + params, model_type = _collision_params(model, mesh_parameters) + body = CollisionBody(pose, _graspable_metadata(state, source, stamp)) + rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params) -@handle(SceneServiceException, logger, message="Failed to get graspable states.") -def graspable_states() -> dict[str, str]: - states: dict[str, str] = {} - for gid in graspable_ids(): - payload = rest.call(rest.Method.GET, f"{URL}/graspables/{gid}", return_type=dict) - state = payload.get("state") - if not isinstance(state, str): - raise SceneServiceException(f"Graspable {gid} has invalid or missing 'state'.") - states[gid] = state - return states +# TODO: change endpoint /collisions/... ? +def reserve_nearest_graspable(position: Position, radius: float) -> str: + return rest.call( + rest.Method.PUT, f"{URL}/graspable/reserve-nearest", body=ReserveGraspable(position, radius), return_type=str + ) -@handle(SceneServiceException, logger, message="Failed to delete the graspable.") -def delete_graspable_id(graspable_id: str) -> None: - rest.call(rest.Method.DELETE, f"{URL}/graspables/{graspable_id}") +@handle(SceneServiceException, logger, message="Failed to delete the collision.") +def delete_collision_id(collision_id: str) -> None: + rest.call(rest.Method.DELETE, f"{URL}/collisions/{collision_id}") -def delete_all_graspable() -> None: - for cid in collision_ids(): - delete_collision_id(cid) +@handle(SceneServiceException, logger, message="Failed to list collisions.") +def collision_ids() -> set[str]: + return set(rest.call(rest.Method.GET, f"{URL}/collisions", list_return_type=str)) @handle(SceneServiceException, logger, message="Failed to focus the object.") @@ -215,6 +247,7 @@ def world_pose(transform_id: str) -> Pose: __all__ = [ SceneServiceException.__name__, upsert_collision.__name__, + upsert_graspable.__name__, delete_collision_id.__name__, collision_ids.__name__, focus.__name__, @@ -225,9 +258,5 @@ def world_pose(transform_id: str) -> Pose: upsert_transform.__name__, local_pose.__name__, world_pose.__name__, - upsert_graspable.__name__, - delete_graspable_id.__name__, - graspable_ids.__name__, - delete_all_graspable.__name__, - graspable_states.__name__, + reserve_nearest_graspable.__name__, ] diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index 5dab40a4..45933788 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -4,7 +4,7 @@ from dataclasses_jsonschema import JsonSchemaMixin -from arcor2.data.common import ActionMetadata, Joint, Pose, StrEnum +from arcor2.data.common import ActionMetadata, Joint, Pose, Position, StrEnum from arcor2.data.robot import InverseKinematicsRequest from arcor2_object_types.abstract import Robot, Settings from arcor2_web import rest @@ -24,6 +24,13 @@ def avg(self) -> float: return (self.a + self.b) / 2 +@dataclass +class MoveObject(JsonSchemaMixin): + object_id: str + effector_type: str + pose: Pose + + class VacuumChannel(StrEnum): A = "a" B = "b" @@ -113,6 +120,14 @@ def move( params={"velocity": speed, "payload": payload, "safe": safe}, ) + def move_object_to_pose(self, object_id: str, effector_type: str, pose: Pose) -> None: + with self._move_lock: + rest.call( + rest.Method.PUT, + f"{self.settings.url}/graspable/move", + body=MoveObject(object_id, effector_type, pose), + ) + def suck( self, vacuum: int = 60, diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index 7aaadd50..a65c99a8 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -1,24 +1,21 @@ import importlib import multiprocessing -import struct -import sys import threading import time +from dataclasses import dataclass, field from multiprocessing.connection import Connection from typing import Any, Callable, NamedTuple, cast import rclpy # pants: no-infer-dep -from geometry_msgs.msg import PoseStamped # pants: no-infer-dep -from geometry_msgs.msg import Point from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep +from geometry_msgs.msg import PoseStamped # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep from moveit_msgs.msg import CollisionObject # pants: no-infer-dep from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep from sensor_msgs.msg import JointState # pants: no-infer-dep -from shape_msgs.msg import Mesh as RosMesh # pants: no-infer-dep -from shape_msgs.msg import MeshTriangle, SolidPrimitive +from shape_msgs.msg import SolidPrimitive # pants: no-infer-dep from std_msgs.msg import Bool, String # pants: no-infer-dep from std_srvs.srv import Trigger # pants: no-infer-dep from tf2_ros.buffer import Buffer # pants: no-infer-dep @@ -29,7 +26,7 @@ from arcor2 import transformations as tr from arcor2.data import common, object_type -from arcor2.data.common import Joint, Pose +from arcor2.data.common import Joint, Orientation, Pose, Position from arcor2.data.robot import InverseKinematicsRequest from arcor2.logging import get_logger from arcor2_ur import topics @@ -44,17 +41,11 @@ WORKSPACE_MAX = (1.0, 1.0, 1.0) -class CollisionObjectTuple(NamedTuple): +@dataclass +class CollisionSceneObject: model: object_type.Models pose: common.Pose - - -class GraspableObjectTuple(NamedTuple): - model: object_type.Models - pose: common.Pose - state: str - source: str - stamp: str + metadata: dict[str, Any] = field(default_factory=dict) def pose_to_ros_pose(ps: Pose) -> RosPose: @@ -107,6 +98,86 @@ def wait_for_future(future, *, timeout_sec: float = 2.0): return future.result() +def create_collision_object(obj: CollisionSceneObject, obj_id: str, base_link: str, base_pose: Pose) -> CollisionObject: + collision_object = CollisionObject() + collision_object.header.frame_id = base_link + collision_object.id = obj_id + + obj_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) + prim = SolidPrimitive() + + if isinstance(obj.model, object_type.Box): + prim.type = SolidPrimitive.BOX + prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] + + elif isinstance(obj.model, object_type.Sphere): + prim.type = SolidPrimitive.SPHERE + prim.dimensions = [obj.model.radius] + + elif isinstance(obj.model, object_type.Cylinder): + prim.type = SolidPrimitive.CYLINDER + prim.dimensions = [obj.model.height, obj.model.radius] + + # elif isinstance(obj.model, object_type.Mesh): + + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(obj_pose) + + collision_object.operation = CollisionObject.ADD + return collision_object + + +def apply_collision_objects( + scene, collision_objects: dict[str, CollisionSceneObject], base_pose: Pose, base_link: str +) -> None: + scene.remove_all_collision_objects() # just to be sure that scene is clean for update + + for obj_id, obj in collision_objects.items(): + if obj.metadata.get("state") in ("ATTACHED", "LOST"): + continue + + collision_object = create_collision_object(obj, obj_id, base_link, base_pose) + collision_object.operation = CollisionObject.ADD + scene.apply_collision_object(collision_object) + + scene.current_state.update() + + +def generate_grasp_poses(object: CollisionSceneObject, effector_type: str) -> list[tuple[Pose, Pose]]: + grasp_poses: list[tuple[Pose, Pose]] = [] + + grasp_offset = 0.01 + pre_grasp_offset = 0.10 + + x = object.pose.position.x + y = object.pose.position.y + z = object.pose.position.z + + if effector_type == "suck": + + if isinstance(object.model, object_type.Box): + top_z = z + object.model.size_z / 2 + + # top grasp pose + orientation = Orientation(0, 0, 0, 1) + grasp_pose = Pose(Position(x, y, top_z + grasp_offset), orientation) + pre_grasp_pose = Pose(Position(x, y, top_z + pre_grasp_offset), orientation) + grasp_poses.append((pre_grasp_pose, grasp_pose)) + + # TODO: side points + + elif isinstance(object.model, object_type.Cylinder): + pass + + elif isinstance(object.model, object_type.Sphere): + pass + + elif isinstance(object.model, object_type.Mesh): + pass + + return grasp_poses + + class MyNode(Node): def __init__( self, @@ -309,68 +380,13 @@ def joint_states_cb(self, msg: JointState) -> None: self._joint_state_handler(list(msg.name), list(msg.position)) -def create_collision_object(obj, obj_id, base_link: str, base_pose: Pose) -> CollisionObject: - collision_object = CollisionObject() - collision_object.header.frame_id = base_link - collision_object.id = obj_id - - obj_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) - prim = SolidPrimitive() - - if isinstance(obj.model, object_type.Box): - prim.type = SolidPrimitive.BOX - prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] - - elif isinstance(obj.model, object_type.Sphere): - prim.type = SolidPrimitive.SPHERE - prim.dimensions = [obj.model.radius] - - elif isinstance(obj.model, object_type.Cylinder): - prim.type = SolidPrimitive.CYLINDER - prim.dimensions = [obj.model.height, obj.model.radius] - - # elif isinstance(obj.model, object_type.Mesh): - - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(obj_pose) - - collision_object.operation = CollisionObject.ADD - return collision_object - - -def apply_collision_objects( - scene, - collision_objects: dict[str, CollisionObjectTuple], - graspable_objects: dict[str, GraspableObjectTuple], - base_pose: Pose, - base_link: str, -) -> None: - scene.remove_all_collision_objects() # just to be sure that scene is clean for update - - for obj_id, obj in collision_objects.items(): - - collision_object = create_collision_object(obj, obj_id, base_link, base_pose) - collision_object.operation = CollisionObject.ADD - scene.apply_collision_object(collision_object) - - for obj_id, obj in graspable_objects.items(): - if obj.state == "WORLD": # graspable objects which are not moving - - collision_object = create_collision_object(obj, obj_id, base_link, base_pose) - collision_object.operation = CollisionObject.ADD - scene.apply_collision_object(collision_object) - - scene.current_state.update() - - class RosWorkerRuntime: def __init__( self, base_pose: Pose, base_link: str, tool_link: str, - collision_objects: dict[str, CollisionObjectTuple], - graspable_objects: dict[str, GraspableObjectTuple], + collision_objects: dict[str, CollisionSceneObject], interact_with_dashboard: bool, robot_ip: str, vgc10_port: int, @@ -381,10 +397,7 @@ def __init__( self.base_pose = base_pose self.base_link = base_link self.tool_link = tool_link - self.collision_objects = collision_objects.copy() - self.graspable_objects = graspable_objects.copy() - self.joints: list[Joint] = [] self.tool: VGC10 | None = None self.freedrive_mode = False @@ -446,13 +459,7 @@ def __init__( moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects( - scene, - collision_objects=self.collision_objects, - graspable_objects=self.graspable_objects, - base_pose=self.base_pose, - base_link=self.base_link, - ) + apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) def _update_joints(self, names: list[str], positions: list[float]) -> None: self.joints = [Joint(name, position) for name, position in zip(names, positions)] @@ -646,13 +653,7 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) moveitpy, ur_manipulator = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if safe: - apply_collision_objects( - scene, - collision_objects=self.collision_objects, - graspable_objects=self.graspable_objects, - base_pose=self.base_pose, - base_link=self.base_link, - ) + apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) else: scene.remove_all_collision_objects() @@ -689,13 +690,7 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]: moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if ikr.avoid_collisions: - apply_collision_objects( - scene, - collision_objects=self.collision_objects, - graspable_objects=self.graspable_objects, - base_pose=self.base_pose, - base_link=self.base_link, - ) + apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) else: scene.remove_all_collision_objects() @@ -718,13 +713,7 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]: raise UrGeneral("State is in collision.") except UrGeneral: if not ikr.avoid_collisions: - apply_collision_objects( - scene, - collision_objects=self.collision_objects, - graspable_objects=self.graspable_objects, - base_pose=self.base_pose, - base_link=self.base_link, - ) + apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) raise if len(scene.current_state.joint_positions) != len(self.joints): @@ -741,30 +730,11 @@ def set_freedrive_mode(self, enabled: bool) -> dict: def get_freedrive_mode(self) -> bool: return self.freedrive_mode - def update_collisions(self, collision_objects: dict[str, CollisionObjectTuple]) -> dict: + def update_collisions(self, collision_objects: dict[str, CollisionSceneObject]) -> dict: self.collision_objects = collision_objects.copy() moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects( - scene, - collision_objects=self.collision_objects, - graspable_objects=self.graspable_objects, - base_pose=self.base_pose, - base_link=self.base_link, - ) - return {} - - def update_graspables(self, graspable_objects: dict[str, GraspableObjectTuple]) -> dict: - self.graspable_objects = graspable_objects.copy() - moveitpy, _ = self._get_moveit() - with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects( - scene, - collision_objects=self.collision_objects, - graspable_objects=self.graspable_objects, - base_pose=self.base_pose, - base_link=self.base_link, - ) + apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) return {} def suck(self, vacuum: int) -> dict: @@ -792,14 +762,42 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]: raise UrGeneral("MoveIt is not initialized.") return self.moveitpy, self.ur_manipulator + def attach_object(self, effector_type, grasp_pose, object) -> None: + # TODO: add suction / close gripper + + self.move_to_pose(grasp_pose) + return + + def dettach_object(self, object) -> None: + # TODO: turn off suction / open gripper + return + + def move_object_to_pose(self, object_id: str, effector_type: str, target_pose: Pose) -> None: + object = self.collision_objects[object_id] + + grasp_options = generate_grasp_poses(object, effector_type) + + for pre_grasp_pose, grasp_pose in grasp_options: + if not self.move_to_pose(pre_grasp_pose): + continue + + if not self.attach_object(effector_type, grasp_pose, object): + continue + + self.move_to_pose(target_pose) + self.dettach_object(object) + + return + + return + def ros_worker_main( conn: Connection, base_pose: Pose, base_link: str, tool_link: str, - collision_objects: dict[str, CollisionObjectTuple], - graspable_objects: dict[str, GraspableObjectTuple], + collision_objects: dict[str, CollisionSceneObject], interact_with_dashboard: bool, robot_ip: str, vgc10_port: int, @@ -815,7 +813,6 @@ def ros_worker_main( base_link, tool_link, collision_objects, - graspable_objects, interact_with_dashboard, robot_ip, vgc10_port, @@ -824,7 +821,7 @@ def ros_worker_main( debug, ) conn.send({"status": "ok"}) - except Exception as exc: # pragma: no cover + except Exception as exc: # pragma: no cover - best effort to report start failure logger.exception("Failed to start ROS worker.") try: conn.send({"status": "error", "message": str(exc)}) @@ -859,6 +856,9 @@ def ros_worker_main( elif op == "move_to_pose": pose = Pose.from_dict(kwargs["pose"]) result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"]) + elif op == "move_object_to_pose": + pose = Pose.from_dict(kwargs["pose"]) + result = runtime.move_object_to_pose(kwargs["object_id"], kwargs["effector_type"], pose) elif op == "get_joints": result = runtime.get_joints() elif op == "ik": @@ -869,8 +869,6 @@ def ros_worker_main( result = runtime.get_freedrive_mode() elif op == "update_collisions": result = runtime.update_collisions(kwargs["collision_objects"]) - elif op == "update_graspables": - result = runtime.update_graspables(kwargs["graspable_objects"]) elif op == "suck": result = runtime.suck(kwargs["vacuum"]) elif op == "release": @@ -900,8 +898,7 @@ class RosWorkerClient: def __init__( self, pose: Pose, - collision_objects: dict[str, CollisionObjectTuple], - graspable_objects: dict[str, GraspableObjectTuple], + collision_objects: dict[str, CollisionSceneObject], base_link: str, tool_link: str, planning_group_name: str, @@ -921,7 +918,6 @@ def __init__( base_link, tool_link, collision_objects, - graspable_objects, interact_with_dashboard, robot_ip, vgc10_port, diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index 19b2e053..e179e197 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -5,6 +5,8 @@ import os from dataclasses import dataclass, field from functools import wraps +from math import dist +from typing import Any import humps from ament_index_python.packages import get_package_share_directory # pants: no-infer-dep @@ -17,10 +19,11 @@ from arcor2.data.robot import InverseKinematicsRequest from arcor2.helpers import port_from_url from arcor2.logging import get_logger +from arcor2_object_types.abstract import GraspableState from arcor2_ur import get_data, version from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum -from arcor2_ur.scripts.ros_worker import CollisionObjectTuple, GraspableObjectTuple, RosWorkerClient +from arcor2_ur.scripts.ros_worker import CollisionSceneObject, RosWorkerClient from arcor2_web.flask import RespT, create_app, run_app logger = get_logger(__name__) @@ -47,18 +50,10 @@ class ServiceState: class Globs: debug = False state: ServiceState | None = None - collision_objects: dict[str, CollisionObjectTuple] = field(default_factory=dict) - graspable_objects: dict[str, GraspableObjectTuple] = field(default_factory=dict) + collision_objects: dict[str, CollisionSceneObject] = field(default_factory=dict) scene_started = False # flag for "Scene service" -def _graspable_meta_from_request() -> tuple[str, str, str]: - state = request.args.get("state", default="WORLD") - source = request.args.get("source", default="other") - stamp = request.args.get("stamp", default="") - return state, source, stamp - - globs: Globs = Globs() app = create_app(__name__) @@ -97,6 +92,17 @@ def wrapped(*args, **kwargs): return wrapped +def parse_collision_body() -> tuple[common.Pose, dict[str, Any]]: + body = humps.decamelize(request.json) + + if "pose" in body: + pose = common.Pose.from_dict(body["pose"]) + metadata = body.get("metadata", {}) or {} + return pose, metadata + + return common.Pose.from_dict(body), {} + + @app.route("/system/start", methods=["PUT"]) # for compatibility with Scene service def put_start_scene() -> RespT: """Start the scene (compatibility).""" @@ -137,7 +143,6 @@ def put_start() -> RespT: worker = RosWorkerClient( pose, globs.collision_objects, - globs.graspable_objects, BASE_LINK, TOOL_LINK, PLANNING_GROUP_NAME, @@ -201,6 +206,145 @@ def get_started() -> RespT: return jsonify(started()) +@app.route("/graspable/reserve-nearest", methods=["PUT"]) +def put_reserve_nearest_graspable() -> RespT: + """Reserve nearest graspable object in WORLD state. + --- + put: + tags: + - Graspable + description: Find the nearest graspable object in WORLD state within the given radius, mark it as RESERVED, and return its ID. If no such object is found, returns null. + requestBody: + content: + application/json: + schema: + type: object + required: + - position + - radius + properties: + position: + $ref: Position + radius: + type: number + format: float + responses: + 200: + description: ID of the reserved graspable object, or null if no matching object was found. + content: + application/json: + schema: + oneOf: + - type: string + - type: "null" + 500: + description: "Error types: **General**, **UrGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing position and radius.") + + body = humps.decamelize(request.json) + + try: + position = common.Position.from_dict(body["position"]) + radius = float(body["radius"]) + except KeyError as exc: + raise UrGeneral(f"Missing key: {exc.args[0]}") + + if radius < 0.0: + raise UrGeneral("Radius has to be >= 0.") + + nearest_id: str | None = None + nearest_distance: float | None = None + + for obj_id, obj in globs.collision_objects.items(): + + # only graspable objects + if obj.metadata.get("object_type") != "graspable": + continue + + # only WORLD objects + if obj.metadata.get("state") != GraspableState.WORLD.value: + continue + + current_distance = position.distance(obj.pose.position) + + if current_distance >= radius: + continue + + if nearest_distance is None or current_distance < nearest_distance: + nearest_id = obj_id + nearest_distance = current_distance + + if nearest_id is None: + return jsonify("") + + # change object state to RESERVED + globs.collision_objects[nearest_id].metadata["state"] = GraspableState.RESERVED.value + + # refresh worker + if started(): + assert globs.state + globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) + + return jsonify(nearest_id) + + +@app.route("/graspable/move", methods=["PUT"]) +@requires_started +def move_object() -> RespT: + """Moves graspable object using robot. + --- + put: + tags: + - Graspable + summary: Move graspable object to target pose. + requestBody: + required: true + content: + application/json: + schema: + type: object + required: + - object_id + - effector_type + - pose + properties: + object_id: + type: string + effector_type: + type: string + pose: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**." + content: + application/json: + schema: + $ref: WebApiError + """ + + body = humps.decamelize(request.json) + + object_id = body["object_id"] + effector_type = body["effector_type"] + pose = Pose.from_dict(body["pose"]) + + assert globs.state + globs.state.worker.request( + "move_object_to_pose", object_id=object_id, effector_type=effector_type, pose=pose.to_dict() + ) + + return Response(status=204) + + @app.route("/collisions/box", methods=["PUT"]) def put_box() -> RespT: """Add or update collision box. @@ -246,12 +390,12 @@ def put_box() -> RespT: schema: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise UrGeneral("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"])) - globs.collision_objects[box.id] = CollisionObjectTuple(box, common.Pose.from_dict(humps.decamelize(request.json))) + + globs.collision_objects[box.id] = CollisionSceneObject(box, pose, metadata) if started(): assert globs.state @@ -295,16 +439,12 @@ def put_sphere() -> RespT: schema: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise UrGeneral("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"])) - globs.collision_objects[sphere.id] = CollisionObjectTuple( - sphere, common.Pose.from_dict(humps.decamelize(request.json)) - ) - logger.warning("Sphere collision object added but will be ignored as only boxes are supported at the moment.") + globs.collision_objects[sphere.id] = CollisionSceneObject(sphere, pose, metadata) if started(): assert globs.state @@ -357,16 +497,12 @@ def put_cylinder() -> RespT: schema: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise UrGeneral("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"])) - globs.collision_objects[cylinder.id] = CollisionObjectTuple( - cylinder, common.Pose.from_dict(humps.decamelize(request.json)) - ) - logger.warning("Cylinder collision object added but will be ignored as only boxes are supported at the moment.") + globs.collision_objects[cylinder.id] = CollisionSceneObject(cylinder, pose, metadata) if started(): assert globs.state @@ -427,14 +563,14 @@ def put_mesh() -> RespT: schema: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise UrGeneral("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"]) - globs.collision_objects[mesh.id] = CollisionObjectTuple(mesh, common.Pose.from_dict(humps.decamelize(request.json))) - logger.warning("Mesh collision object added but will be ignored as only boxes are supported at the moment.") + globs.collision_objects[mesh.id] = CollisionSceneObject(mesh, pose, metadata) + + logger.warning("Mesh ignored (only boxes supported).") if started(): assert globs.state @@ -811,445 +947,6 @@ def put_release() -> RespT: return Response(status=204) -@app.route("/graspables/box", methods=["PUT"]) -def put_graspable_box() -> RespT: - """Add or update graspable box. - --- - put: - tags: - - Graspables - description: Add or update graspable box. - parameters: - - name: boxId - in: query - description: unique graspable box ID - required: true - schema: - type: string - - name: sizeX - in: query - schema: - type: number - format: float - - name: sizeY - in: query - schema: - type: number - format: float - - name: sizeZ - in: query - schema: - type: number - format: float - - name: state - in: query - schema: - type: string - default: WORLD - - name: source - in: query - schema: - type: string - default: other - - name: stamp - in: query - schema: - type: string - default: "" - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 204: - description: Ok - 500: - description: "Error types: **General**, **UrGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - if not isinstance(request.json, dict): - raise UrGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = request.args.to_dict() - box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"])) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - globs.graspable_objects[box.id] = GraspableObjectTuple( - model=box, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - # collision-like update logic, but for graspables - if started(): - assert globs.state - globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) - - return Response(status=204) - - -@app.route("/graspables/sphere", methods=["PUT"]) -def put_graspable_sphere() -> RespT: - """Add or update graspable sphere. - --- - put: - tags: - - Graspables - description: Add or update graspable sphere. - parameters: - - name: sphereId - in: query - description: unique graspable sphere ID - required: true - schema: - type: string - - name: radius - in: query - schema: - type: number - format: float - - name: state - in: query - schema: - type: string - default: WORLD - - name: source - in: query - schema: - type: string - default: other - - name: stamp - in: query - schema: - type: string - default: "" - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 204: - description: Ok - 500: - description: "Error types: **General**, **UrGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - if not isinstance(request.json, dict): - raise UrGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = humps.decamelize(request.args.to_dict()) - sphere = object_type.Sphere(args["sphere_id"], float(args["radius"])) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - globs.graspable_objects[sphere.id] = GraspableObjectTuple( - model=sphere, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - if started(): - assert globs.state - globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) - - return Response(status=204) - - -@app.route("/graspables/cylinder", methods=["PUT"]) -def put_graspable_cylinder() -> RespT: - """Add or update graspable cylinder. - --- - put: - tags: - - Graspables - description: Add or update graspable cylinder. - parameters: - - name: cylinderId - in: query - description: unique graspable cylinder ID - required: true - schema: - type: string - - name: radius - in: query - schema: - type: number - format: float - - name: height - in: query - schema: - type: number - format: float - - name: state - in: query - schema: - type: string - default: WORLD - - name: source - in: query - schema: - type: string - default: other - - name: stamp - in: query - schema: - type: string - default: "" - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 204: - description: Ok - 500: - description: "Error types: **General**, **UrGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - if not isinstance(request.json, dict): - raise UrGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = humps.decamelize(request.args.to_dict()) - cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"])) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - globs.graspable_objects[cylinder.id] = GraspableObjectTuple( - model=cylinder, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - if started(): - assert globs.state - globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) - - return Response(status=204) - - -@app.route("/graspables/mesh", methods=["PUT"]) -def put_graspable_mesh() -> RespT: - """Add or update graspable mesh. - --- - put: - tags: - - Graspables - description: Add or update graspable mesh. - parameters: - - name: meshId - in: query - description: unique graspable mesh ID - required: true - schema: - type: string - - name: meshFileId - in: query - schema: - type: string - - name: meshScaleX - in: query - schema: - type: number - format: float - default: 1.0 - - name: meshScaleY - in: query - schema: - type: number - format: float - default: 1.0 - - name: meshScaleZ - in: query - schema: - type: number - format: float - default: 1.0 - - name: state - in: query - schema: - type: string - default: WORLD - - name: source - in: query - schema: - type: string - default: other - - name: stamp - in: query - schema: - type: string - default: "" - requestBody: - content: - application/json: - schema: - $ref: Pose - responses: - 204: - description: Ok - 500: - description: "Error types: **General**, **UrGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - if not isinstance(request.json, dict): - raise UrGeneral("Body should be a JSON dict containing Pose.") - - state, source, stamp = _graspable_meta_from_request() - - args = humps.decamelize(request.args.to_dict()) - mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) - pose = common.Pose.from_dict(humps.decamelize(request.json)) - - globs.graspable_objects[mesh.id] = GraspableObjectTuple( - model=mesh, - pose=pose, - state=state, - source=source, - stamp=stamp, - ) - - if started(): - assert globs.state - globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) - - return Response(status=204) - - -@app.route("/graspables/", methods=["DELETE"]) -def delete_graspable(id: str) -> RespT: - """Deletes graspable object. - --- - delete: - tags: - - Graspables - summary: Deletes graspable object. - parameters: - - name: id - in: path - description: unique graspable ID - required: true - schema: - type: string - responses: - 204: - description: Ok - 500: - description: "Error types: **General**, **NotFound**." - content: - application/json: - schema: - $ref: WebApiError - """ - try: - del globs.graspable_objects[id] - except KeyError: - raise NotFound("Graspable not found") - - if started(): - assert globs.state - globs.state.worker.request("update_graspables", graspable_objects=globs.graspable_objects) - - return Response(status=204) - - -@app.route("/graspables", methods=["GET"]) -def get_graspables() -> RespT: - """Gets graspable ids. - --- - get: - tags: - - Graspables - summary: Gets graspable ids. - responses: - 200: - description: Success - content: - application/json: - schema: - type: array - items: - type: string - 500: - description: "Error types: **General**." - content: - application/json: - schema: - $ref: WebApiError - """ - return jsonify(list(globs.graspable_objects.keys())) - - -@app.route("/graspables/", methods=["GET"]) -def get_graspable(id: str) -> RespT: - """Gets graspable detail. - --- - get: - tags: - - Graspables - summary: Gets graspable detail. - parameters: - - name: id - in: path - description: unique graspable ID - required: true - schema: - type: string - responses: - 200: - description: Success - 500: - description: "Error types: **General**, **NotFound**." - content: - application/json: - schema: - $ref: WebApiError - """ - try: - entry = globs.graspable_objects[id] - except KeyError: - raise NotFound("Graspable not found") - - model = entry.model - pose = entry.pose - - return jsonify( - { - "id": id, - "modelType": model.type().value.lower(), - "model": model.to_dict(), - "pose": pose.to_dict(), - "state": entry.state, - "source": entry.source, - "stamp": entry.stamp, - } - ) - - def main() -> None: parser = argparse.ArgumentParser(description=SERVICE_NAME) parser.add_argument("-s", "--swagger", action="store_true", default=False) diff --git a/src/python/arcor2_ur/tests/test_box_in_path.py b/src/python/arcor2_ur/tests/test_box_in_path.py deleted file mode 100644 index 8a5ef730..00000000 --- a/src/python/arcor2_ur/tests/test_box_in_path.py +++ /dev/null @@ -1,53 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Box -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_box_in_path(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - box = Box("Box1", 0.2, 0.2, 0.95) - scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box2", 0.2, 0.2, 0.95) - scene_service.upsert_collision(box, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_box_up.py b/src/python/arcor2_ur/tests/test_box_up.py deleted file mode 100644 index 8b983dd3..00000000 --- a/src/python/arcor2_ur/tests/test_box_up.py +++ /dev/null @@ -1,63 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Box -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_box_up(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - box = Box("Box1", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box2", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box3", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box4", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_cylinder_in_path.py b/src/python/arcor2_ur/tests/test_cylinder_in_path.py deleted file mode 100644 index 6c830da9..00000000 --- a/src/python/arcor2_ur/tests/test_cylinder_in_path.py +++ /dev/null @@ -1,53 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Cylinder -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_cylinder_in_path(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - cyl = Cylinder("Cyl1", 0.1, 0.95) - scene_service.upsert_collision(cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl2", 0.1, 0.95) - scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_cylinder_up.py b/src/python/arcor2_ur/tests/test_cylinder_up.py deleted file mode 100644 index 5c957b83..00000000 --- a/src/python/arcor2_ur/tests/test_cylinder_up.py +++ /dev/null @@ -1,63 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Cylinder -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_cylinder_up(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - cyl = Cylinder("Cyl1", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl2", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl3", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl4", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_grasapble_collizion_object.py b/src/python/arcor2_ur/tests/test_grasapble_collizion_object.py deleted file mode 100644 index 723dea20..00000000 --- a/src/python/arcor2_ur/tests/test_grasapble_collizion_object.py +++ /dev/null @@ -1,62 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Cylinder -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_sphere_up(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - cyl = Cylinder("Cyl1", 0.1, 0.95) - scene_service.upsert_graspable( - cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), state="WORLD", source="test" - ) - assert cyl.id in scene_service.graspable_ids() - time.sleep(1) - - cyl = Cylinder("Cyl2", 0.1, 0.95) - scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl3", 0.1, 0.95) - scene_service.upsert_graspable( - cyl, Pose(Position(0.0, 0.0, 0.0), Orientation(0, 0, 0, 1)), state="ATTACHED", source="test" - ) - assert cyl.id in scene_service.graspable_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_graspable_collision_object.py b/src/python/arcor2_ur/tests/test_graspable_collision_object.py new file mode 100644 index 00000000..af23e48e --- /dev/null +++ b/src/python/arcor2_ur/tests/test_graspable_collision_object.py @@ -0,0 +1,59 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_object_types.abstract import GraspableSource, GraspableState +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_graspable_collision_object(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + y = 0.0 + z = 0.2 + x = 0.9 + + start_pose = Pose(Position(x, y, z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(y, x, z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl1 = Cylinder("Cyl1", 0.1, 0.95) + scene_service.upsert_graspable( + cyl1, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), state=GraspableState.WORLD + ) + assert cyl1.id in scene_service.collision_ids() + time.sleep(1) + + cyl2 = Cylinder("Cyl2", 0.1, 0.95) + scene_service.upsert_collision(cyl2, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl2.id in scene_service.collision_ids() + time.sleep(1) + + cyl3 = Cylinder("Cyl3", 0.1, 0.95) + scene_service.upsert_graspable( + cyl3, + Pose(Position(0.0, 0.0, 0.0), Orientation(0, 0, 0, 1)), + state=GraspableState.ATTACHED, + source=GraspableSource.OTHER, + ) + assert cyl3.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_overhead_obstacle.py new file mode 100644 index 00000000..3fdebb82 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_overhead_obstacle.py @@ -0,0 +1,148 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box, Cylinder, Sphere +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_box(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box2", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box3", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box4", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() + + +@pytest.mark.timeout(321) +def test_cylinder(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl3", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl4", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() + + +@pytest.mark.timeout(321) +def test_sphere(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere2", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere3", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere4", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_reserve_object.py b/src/python/arcor2_ur/tests/test_reserve_object.py new file mode 100644 index 00000000..d78ffdd1 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_reserve_object.py @@ -0,0 +1,49 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_object_types.abstract import GraspableSource, GraspableState +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_reserve_object(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + + cyl1 = Cylinder("Cyl1", 0.1, 0.95) + scene_service.upsert_graspable( + cyl1, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), state=GraspableState.WORLD + ) + assert cyl1.id in scene_service.collision_ids() + time.sleep(1) + + cyl2 = Cylinder("Cyl2", 0.1, 0.95) + scene_service.upsert_collision(cyl2, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl2.id in scene_service.collision_ids() + time.sleep(1) + + cyl3 = Cylinder("Cyl3", 0.1, 0.95) + scene_service.upsert_graspable( + cyl3, + Pose(Position(0.0, 0.0, 0.0), Orientation(0, 0, 0, 1)), + state=GraspableState.WORLD, + source=GraspableSource.OTHER, + ) + assert cyl3.id in scene_service.collision_ids() + time.sleep(1) + + assert cyl1.id == scene_service.reserve_nearest_graspable(Position(0.5, 0.5, 0.5), 0.05) + assert cyl3.id == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.05) + assert "" == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.05) + + scene_service.delete_all_collisions() + + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_side_obstacle.py b/src/python/arcor2_ur/tests/test_side_obstacle.py new file mode 100644 index 00000000..47e88f46 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_side_obstacle.py @@ -0,0 +1,138 @@ +import math +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box, Cylinder, Sphere +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_box(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box1", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box2", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() + + +@pytest.mark.timeout(321) +def test_cylinder(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() + + +@pytest.mark.timeout(321) +def test_sphere(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + Y = 0.0 + Z = 0.2 + X = 0.9 + + start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) + goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere2", 0.1) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere3", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere4", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere5", 0.1) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere6", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_sphere_in_path.py b/src/python/arcor2_ur/tests/test_sphere_in_path.py deleted file mode 100644 index 12f8df6a..00000000 --- a/src/python/arcor2_ur/tests/test_sphere_in_path.py +++ /dev/null @@ -1,73 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Sphere -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_sphere_in_path(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - sphere = Sphere("Sphere1", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere2", 0.1) - scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere3", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere4", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere5", 0.1) - scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere6", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_sphere_up.py b/src/python/arcor2_ur/tests/test_sphere_up.py deleted file mode 100644 index 25f8649b..00000000 --- a/src/python/arcor2_ur/tests/test_sphere_up.py +++ /dev/null @@ -1,63 +0,0 @@ -import math -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Sphere -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -def _dist(a: Pose, b: Pose) -> float: - dx = a.position.x - b.position.x - dy = a.position.y - b.position.y - dz = a.position.z - b.position.z - return math.sqrt(dx * dx + dy * dy + dz * dz) - - -@pytest.mark.timeout(220) -def test_sphere_up(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - Y = 0.0 - Z = 0.2 - X = 0.9 - - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - sphere = Sphere("Sphere1", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere2", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere3", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere4", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert _dist(reached, goal_pose) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() From 8cbaa880cb5bd3136f460103cf882136e305b0e8 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Tue, 31 Mar 2026 17:24:38 +0200 Subject: [PATCH 04/11] feat(graspable): add basic object transfer (move_object_to_pose) --- src/python/arcor2_scene/scripts/scene.py | 4 +- src/python/arcor2_ur/common.py | 25 +++ src/python/arcor2_ur/data/urdf/ur5e.urdf | 81 +++++++-- src/python/arcor2_ur/object_types/ur5e.py | 25 ++- src/python/arcor2_ur/scripts/ros_worker.py | 170 ++++++++++++------ src/python/arcor2_ur/scripts/ur.py | 25 ++- .../arcor2_ur/tests/test_overhead_obstacle.py | 1 - .../arcor2_ur/tests/test_reserve_object.py | 6 +- 8 files changed, 242 insertions(+), 95 deletions(-) create mode 100644 src/python/arcor2_ur/common.py diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index b0a116a9..dac0cae8 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -5,7 +5,6 @@ import math import random import time -from dataclasses import dataclass, field from typing import Any import humps @@ -19,8 +18,7 @@ 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.scripts.ros_worker import CollisionSceneObject -from arcor2_ur.scripts.ur import parse_collision_body +from arcor2_ur.common import CollisionSceneObject, parse_collision_body from arcor2_web.flask import Response, RespT, create_app, run_app app = create_app(__name__) diff --git a/src/python/arcor2_ur/common.py b/src/python/arcor2_ur/common.py new file mode 100644 index 00000000..baa1e06b --- /dev/null +++ b/src/python/arcor2_ur/common.py @@ -0,0 +1,25 @@ +from dataclasses import dataclass, field +from typing import Any + +import humps +from flask import request + +from arcor2.data import common, object_type + + +@dataclass +class CollisionSceneObject: + model: object_type.Models + pose: common.Pose + metadata: dict[str, Any] = field(default_factory=dict) + + +def parse_collision_body() -> tuple[common.Pose, dict[str, Any]]: + body = humps.decamelize(request.json) + + if "pose" in body: + pose = common.Pose.from_dict(body["pose"]) + metadata = body.get("metadata", {}) or {} + return pose, metadata + + return common.Pose.from_dict(body), {} diff --git a/src/python/arcor2_ur/data/urdf/ur5e.urdf b/src/python/arcor2_ur/data/urdf/ur5e.urdf index f0e7f573..f6496a16 100644 --- a/src/python/arcor2_ur/data/urdf/ur5e.urdf +++ b/src/python/arcor2_ur/data/urdf/ur5e.urdf @@ -61,13 +61,13 @@ - + - + @@ -80,13 +80,13 @@ - + - + @@ -99,13 +99,13 @@ - + - + @@ -118,13 +118,13 @@ - + - + @@ -137,13 +137,13 @@ - + - + @@ -156,13 +156,13 @@ - + - + @@ -175,13 +175,13 @@ - + - + @@ -257,7 +257,7 @@ - + @@ -276,7 +276,7 @@ - + @@ -288,4 +288,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index 45933788..ecb069f9 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -29,6 +29,9 @@ class MoveObject(JsonSchemaMixin): object_id: str effector_type: str pose: Pose + velocity: float = 50.0 + payload: float = 0.0 + safe: bool = True class VacuumChannel(StrEnum): @@ -120,12 +123,30 @@ def move( params={"velocity": speed, "payload": payload, "safe": safe}, ) - def move_object_to_pose(self, object_id: str, effector_type: str, pose: Pose) -> None: + def move_object_to_pose( + self, + object_id: str, + effector_type: str, + pose: Pose, + speed: float = 50.0, + safe: bool = True, + payload: float = 0.0, + ) -> None: + assert 0.0 <= speed <= 100.0 + assert 0.0 <= payload <= 5.0 + with self._move_lock: rest.call( rest.Method.PUT, f"{self.settings.url}/graspable/move", - body=MoveObject(object_id, effector_type, pose), + body=MoveObject( + object_id=object_id, + effector_type=effector_type, + pose=pose, + velocity=speed, + payload=payload, + safe=safe, + ), ) def suck( diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index a65c99a8..adcbef68 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -2,15 +2,14 @@ import multiprocessing import threading import time -from dataclasses import dataclass, field from multiprocessing.connection import Connection -from typing import Any, Callable, NamedTuple, cast +from typing import Any, Callable, cast import rclpy # pants: no-infer-dep from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep from geometry_msgs.msg import PoseStamped # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep -from moveit_msgs.msg import CollisionObject # pants: no-infer-dep +from moveit_msgs.msg import AttachedCollisionObject, CollisionObject # pants: no-infer-dep from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep @@ -25,11 +24,13 @@ from ur_msgs.srv import SetPayload, SetSpeedSliderFraction # pants: no-infer-dep from arcor2 import transformations as tr -from arcor2.data import common, object_type +from arcor2.data import object_type from arcor2.data.common import Joint, Orientation, Pose, Position from arcor2.data.robot import InverseKinematicsRequest from arcor2.logging import get_logger +from arcor2_object_types.abstract import GraspableState from arcor2_ur import topics +from arcor2_ur.common import CollisionSceneObject from arcor2_ur.exceptions import UrGeneral from arcor2_ur.object_types.ur5e import Vacuum from arcor2_ur.vgc10 import VGC10 @@ -41,13 +42,6 @@ WORKSPACE_MAX = (1.0, 1.0, 1.0) -@dataclass -class CollisionSceneObject: - model: object_type.Models - pose: common.Pose - metadata: dict[str, Any] = field(default_factory=dict) - - def pose_to_ros_pose(ps: Pose) -> RosPose: rp = RosPose() rp.position.x = ps.position.x @@ -98,12 +92,9 @@ def wait_for_future(future, *, timeout_sec: float = 2.0): return future.result() -def create_collision_object(obj: CollisionSceneObject, obj_id: str, base_link: str, base_pose: Pose) -> CollisionObject: - collision_object = CollisionObject() - collision_object.header.frame_id = base_link - collision_object.id = obj_id - - obj_pose = pose_to_ros_pose(tr.make_pose_rel(base_pose, obj.pose)) +def create_collision_object( + obj: CollisionSceneObject, obj_id: str, frame_id: str, pose_in_frame: Pose, attached_to_link: str | None = None +) -> CollisionObject | AttachedCollisionObject | None: prim = SolidPrimitive() if isinstance(obj.model, object_type.Box): @@ -118,36 +109,31 @@ def create_collision_object(obj: CollisionSceneObject, obj_id: str, base_link: s prim.type = SolidPrimitive.CYLINDER prim.dimensions = [obj.model.height, obj.model.radius] - # elif isinstance(obj.model, object_type.Mesh): + else: + logger.warning(f"Unsupported collision model for MoveIt scene: {obj.model.type()}. Skipping {obj_id}.") + return None + collision_object = CollisionObject() + collision_object.header.frame_id = frame_id + collision_object.id = obj_id collision_object.primitives.append(prim) - collision_object.primitive_poses.append(obj_pose) - + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) collision_object.operation = CollisionObject.ADD - return collision_object + if attached_to_link is not None: + attached = AttachedCollisionObject() + attached.link_name = attached_to_link + attached.object = collision_object + return attached -def apply_collision_objects( - scene, collision_objects: dict[str, CollisionSceneObject], base_pose: Pose, base_link: str -) -> None: - scene.remove_all_collision_objects() # just to be sure that scene is clean for update - - for obj_id, obj in collision_objects.items(): - if obj.metadata.get("state") in ("ATTACHED", "LOST"): - continue - - collision_object = create_collision_object(obj, obj_id, base_link, base_pose) - collision_object.operation = CollisionObject.ADD - scene.apply_collision_object(collision_object) - - scene.current_state.update() + return collision_object def generate_grasp_poses(object: CollisionSceneObject, effector_type: str) -> list[tuple[Pose, Pose]]: grasp_poses: list[tuple[Pose, Pose]] = [] - grasp_offset = 0.01 - pre_grasp_offset = 0.10 + pre_grasp_offset = 0.30 + grasp_offset = 0.20 x = object.pose.position.x y = object.pose.position.y @@ -459,7 +445,7 @@ def __init__( moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + self.apply_collision_objects(scene) def _update_joints(self, names: list[str], positions: list[float]) -> None: self.joints = [Joint(name, position) for name, position in zip(names, positions)] @@ -653,7 +639,7 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) moveitpy, ur_manipulator = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if safe: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + self.apply_collision_objects(scene) else: scene.remove_all_collision_objects() @@ -676,6 +662,37 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) self._wait_for_pose_reached(target_abs) return {} + def apply_collision_objects(self, scene) -> None: + scene.remove_all_collision_objects() + + for obj_id, obj in self.collision_objects.items(): + state = obj.metadata.get("state") + + if state == "LOST": + continue + + # TODO: choose it by id + if state == "ATTACHED": + attached_pose_dict = obj.metadata.get("attached_pose") + if attached_pose_dict is None: + logger.warning(f"Attached object {obj_id} is missing attached_pose. Skipping.") + continue + + pose_in_frame = Pose.from_dict(attached_pose_dict) + attached = create_collision_object(obj, obj_id, self.tool_link, pose_in_frame, self.tool_link) + if attached: + scene.process_attached_collision_object(attached) + continue + + # normal collision object + pose_in_frame = tr.make_pose_rel(self.base_pose, obj.pose) + collision = create_collision_object(obj, obj_id, self.base_link, pose_in_frame) + if collision: + scene.apply_collision_object(collision) + + scene.current_state.update(force=True) + scene.current_state.update() + def get_joints(self) -> list[dict]: return [joint.to_dict() for joint in self.joints] @@ -690,7 +707,7 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]: moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if ikr.avoid_collisions: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + self.apply_collision_objects(scene) else: scene.remove_all_collision_objects() @@ -713,7 +730,7 @@ def inverse_kinematics(self, ikr: InverseKinematicsRequest) -> list[dict]: raise UrGeneral("State is in collision.") except UrGeneral: if not ikr.avoid_collisions: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + self.apply_collision_objects(scene) raise if len(scene.current_state.joint_positions) != len(self.joints): @@ -734,7 +751,7 @@ def update_collisions(self, collision_objects: dict[str, CollisionSceneObject]) self.collision_objects = collision_objects.copy() moveitpy, _ = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: - apply_collision_objects(scene, self.collision_objects, self.base_pose, self.base_link) + self.apply_collision_objects(scene) return {} def suck(self, vacuum: int) -> dict: @@ -762,34 +779,66 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]: raise UrGeneral("MoveIt is not initialized.") return self.moveitpy, self.ur_manipulator - def attach_object(self, effector_type, grasp_pose, object) -> None: - # TODO: add suction / close gripper + def attach_object( + self, + effector_type: str, + grasp_pose: Pose, + object: CollisionSceneObject, + velocity: float, + payload: float, + safe: bool, + ) -> None: - self.move_to_pose(grasp_pose) - return + if effector_type == "suck": + self.move_to_pose(grasp_pose, velocity, payload, safe) + + """ + self.suck(60) + time.sleep(1.0) + + vac = Vacuum.from_dict(self.vacuum()) + if vac.avg() < 20: + self.release() + raise UrGeneral(f"Failed to attach object {object.id}: vacuum not reached.") + """ + object.metadata["state"] = GraspableState.ATTACHED.value + attached_pose = tr.make_pose_rel(grasp_pose, object.pose) + object.metadata["attached_pose"] = attached_pose.to_dict() + + # TODO: kozistencia s ur a scenou + spravnu orientaciu + + else: + raise UrGeneral(f"Unsupported effector type: {effector_type}") - def dettach_object(self, object) -> None: - # TODO: turn off suction / open gripper return - def move_object_to_pose(self, object_id: str, effector_type: str, target_pose: Pose) -> None: + def detach_object(self, object: CollisionSceneObject, target_pose: Pose) -> None: + # self.release() + + object.pose = target_pose + object.metadata["state"] = GraspableState.WORLD.value + object.metadata.pop("attached_pose", None) + + def move_object_to_pose( + self, object_id: str, effector_type: str, target_pose: Pose, velocity: float, payload: float, safe: bool + ) -> None: object = self.collision_objects[object_id] grasp_options = generate_grasp_poses(object, effector_type) for pre_grasp_pose, grasp_pose in grasp_options: - if not self.move_to_pose(pre_grasp_pose): - continue - if not self.attach_object(effector_type, grasp_pose, object): + try: + self.move_to_pose(pre_grasp_pose, velocity, payload, safe) + self.attach_object(effector_type, grasp_pose, object, velocity, payload, False) + except Exception: continue - self.move_to_pose(target_pose) - self.dettach_object(object) - + self.move_to_pose(target_pose, velocity, payload, safe) + self.detach_object(object, target_pose) return - return + raise UrGeneral(f"Unable to move object {object_id} to target pose.") def ros_worker_main( @@ -858,7 +907,14 @@ def ros_worker_main( result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"]) elif op == "move_object_to_pose": pose = Pose.from_dict(kwargs["pose"]) - result = runtime.move_object_to_pose(kwargs["object_id"], kwargs["effector_type"], pose) + result = runtime.move_object_to_pose( + kwargs["object_id"], + kwargs["effector_type"], + pose, + kwargs["velocity"], + kwargs["payload"], + kwargs["safe"], + ) elif op == "get_joints": result = runtime.get_joints() elif op == "ik": diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index e179e197..e4940b72 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -21,9 +21,10 @@ from arcor2.logging import get_logger from arcor2_object_types.abstract import GraspableState from arcor2_ur import get_data, version +from arcor2_ur.common import CollisionSceneObject, parse_collision_body from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum -from arcor2_ur.scripts.ros_worker import CollisionSceneObject, RosWorkerClient +from arcor2_ur.scripts.ros_worker import RosWorkerClient from arcor2_web.flask import RespT, create_app, run_app logger = get_logger(__name__) @@ -92,17 +93,6 @@ def wrapped(*args, **kwargs): return wrapped -def parse_collision_body() -> tuple[common.Pose, dict[str, Any]]: - body = humps.decamelize(request.json) - - if "pose" in body: - pose = common.Pose.from_dict(body["pose"]) - metadata = body.get("metadata", {}) or {} - return pose, metadata - - return common.Pose.from_dict(body), {} - - @app.route("/system/start", methods=["PUT"]) # for compatibility with Scene service def put_start_scene() -> RespT: """Start the scene (compatibility).""" @@ -336,10 +326,19 @@ def move_object() -> RespT: object_id = body["object_id"] effector_type = body["effector_type"] pose = Pose.from_dict(body["pose"]) + velocity = float(body.get("velocity", 50.0)) / 100.0 + payload = float(body.get("payload", 0.0)) + safe = bool(body.get("safe", True)) assert globs.state globs.state.worker.request( - "move_object_to_pose", object_id=object_id, effector_type=effector_type, pose=pose.to_dict() + "move_object_to_pose", + object_id=object_id, + effector_type=effector_type, + pose=pose.to_dict(), + velocity=velocity, + payload=payload, + safe=safe, ) return Response(status=204) diff --git a/src/python/arcor2_ur/tests/test_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_overhead_obstacle.py index 3fdebb82..5c8790b3 100644 --- a/src/python/arcor2_ur/tests/test_overhead_obstacle.py +++ b/src/python/arcor2_ur/tests/test_overhead_obstacle.py @@ -1,4 +1,3 @@ -import math import time import pytest diff --git a/src/python/arcor2_ur/tests/test_reserve_object.py b/src/python/arcor2_ur/tests/test_reserve_object.py index d78ffdd1..d6966d23 100644 --- a/src/python/arcor2_ur/tests/test_reserve_object.py +++ b/src/python/arcor2_ur/tests/test_reserve_object.py @@ -40,9 +40,9 @@ def test_reserve_object(start_processes: Urls) -> None: assert cyl3.id in scene_service.collision_ids() time.sleep(1) - assert cyl1.id == scene_service.reserve_nearest_graspable(Position(0.5, 0.5, 0.5), 0.05) - assert cyl3.id == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.05) - assert "" == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.05) + assert cyl1.id == scene_service.reserve_nearest_graspable(Position(0.5, 0.5, 0.5), 0.5) + assert cyl3.id == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.5) + assert "" == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.5) scene_service.delete_all_collisions() From f289f459183888283286ef23549629161097b157 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Thu, 16 Apr 2026 20:39:17 +0200 Subject: [PATCH 05/11] feat: add graspable first attach and detach algorithm, added stl base a suction cup --- src/python/arcor2_object_types/abstract.py | 5 + src/python/arcor2_scene/scripts/scene.py | 12 +- .../meshes/ur5e/collision/base_link_1cup.stl | 3 + .../ur5e/collision/suction_cup_1cup.stl | 3 + src/python/arcor2_ur/data/urdf/ur5e.urdf | 83 ++---- src/python/arcor2_ur/object_types/ur5e.py | 40 ++- src/python/arcor2_ur/scripts/ros_worker.py | 257 +++++++++++++----- src/python/arcor2_ur/scripts/ur.py | 75 ++++- 8 files changed, 327 insertions(+), 151 deletions(-) create mode 100644 src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl create mode 100644 src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index 7abf8bb1..a6e1665f 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -194,6 +194,10 @@ class GraspableState(StrEnum): 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. @@ -203,6 +207,7 @@ class GraspableState(StrEnum): WORLD = "WORLD" RESERVED = "RESERVED" + HIDEN = "HIDEN" ATTACHED = "ATTACHED" LOST = "LOST" diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index dac0cae8..e73d8429 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -410,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 diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl new file mode 100644 index 00000000..0325ac47 --- /dev/null +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/base_link_1cup.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad35d228922a4e59b18359b84740f77dabb1176050960050be2cb939ee815c7c +size 250084 diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl new file mode 100644 index 00000000..67b71ffa --- /dev/null +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:10686a45842b55d7ce5dc6d3dbf1e51b235a1722e4697e4cec2ccf22e65bbfc4 +size 250034 diff --git a/src/python/arcor2_ur/data/urdf/ur5e.urdf b/src/python/arcor2_ur/data/urdf/ur5e.urdf index f6496a16..77750df7 100644 --- a/src/python/arcor2_ur/data/urdf/ur5e.urdf +++ b/src/python/arcor2_ur/data/urdf/ur5e.urdf @@ -61,13 +61,13 @@ - + - + @@ -80,13 +80,13 @@ - + - + @@ -99,13 +99,13 @@ - + - + @@ -118,13 +118,13 @@ - + - + @@ -137,13 +137,13 @@ - + - + @@ -156,13 +156,13 @@ - + - + @@ -175,13 +175,13 @@ - + - + @@ -257,7 +257,7 @@ - + @@ -276,7 +276,7 @@ - + @@ -288,53 +288,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + \ No newline at end of file diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index ecb069f9..229ad64a 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -25,9 +25,17 @@ def avg(self) -> float: @dataclass -class MoveObject(JsonSchemaMixin): +class AttachObject(JsonSchemaMixin): object_id: str effector_type: str + velocity: float = 50.0 + payload: float = 0.0 + safe: bool = True + + +@dataclass +class DetachObject(JsonSchemaMixin): + object_id: str pose: Pose velocity: float = 50.0 payload: float = 0.0 @@ -123,11 +131,10 @@ def move( params={"velocity": speed, "payload": payload, "safe": safe}, ) - def move_object_to_pose( + def attach_object( self, object_id: str, effector_type: str, - pose: Pose, speed: float = 50.0, safe: bool = True, payload: float = 0.0, @@ -138,10 +145,33 @@ def move_object_to_pose( with self._move_lock: rest.call( rest.Method.PUT, - f"{self.settings.url}/graspable/move", - body=MoveObject( + f"{self.settings.url}/graspable/attach", + body=AttachObject( object_id=object_id, effector_type=effector_type, + velocity=speed, + payload=payload, + safe=safe, + ), + ) + + def detach_object( + self, + object_id: str, + pose: Pose, + speed: float = 50.0, + safe: bool = True, + payload: float = 0.0, + ) -> None: + assert 0.0 <= speed <= 100.0 + assert 0.0 <= payload <= 5.0 + + with self._move_lock: + rest.call( + rest.Method.PUT, + f"{self.settings.url}/graspable/detach", + body=DetachObject( + object_id=object_id, pose=pose, velocity=speed, payload=payload, diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index adcbef68..e821959f 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -1,4 +1,5 @@ import importlib +import math import multiprocessing import threading import time @@ -129,41 +130,137 @@ def create_collision_object( return collision_object -def generate_grasp_poses(object: CollisionSceneObject, effector_type: str) -> list[tuple[Pose, Pose]]: - grasp_poses: list[tuple[Pose, Pose]] = [] +def rotate_vector(q: Orientation, v: tuple[float, float, float]) -> tuple[float, float, float]: + x, y, z, w = q.x, q.y, q.z, q.w + vx, vy, vz = v - pre_grasp_offset = 0.30 - grasp_offset = 0.20 + # quaternion * vector * quaternion_conjugate + # vector berieme ako quaternion (vx, vy, vz, 0) + tx = 2 * (y * vz - z * vy) + ty = 2 * (z * vx - x * vz) + tz = 2 * (x * vy - y * vx) - x = object.pose.position.x - y = object.pose.position.y - z = object.pose.position.z + rx = vx + w * tx + (y * tz - z * ty) + ry = vy + w * ty + (z * tx - x * tz) + rz = vz + w * tz + (x * ty - y * tx) - if effector_type == "suck": + return rx, ry, rz - if isinstance(object.model, object_type.Box): - top_z = z + object.model.size_z / 2 - # top grasp pose - orientation = Orientation(0, 0, 0, 1) - grasp_pose = Pose(Position(x, y, top_z + grasp_offset), orientation) - pre_grasp_pose = Pose(Position(x, y, top_z + pre_grasp_offset), orientation) - grasp_poses.append((pre_grasp_pose, grasp_pose)) +def rotate_effector(a: Orientation, b: Orientation) -> Orientation: + return Orientation( + a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, + ) - # TODO: side points - elif isinstance(object.model, object_type.Cylinder): - pass +def generate_grasp_poses(object: CollisionSceneObject, effector_type: str) -> list[tuple[Pose, Pose]]: + grasp_poses: list[tuple[Pose, Pose]] = [] - elif isinstance(object.model, object_type.Sphere): - pass + pre_grasp_offset = 0.20 + grasp_offset = 0.01 # TODO: -0.01 ? - elif isinstance(object.model, object_type.Mesh): + if effector_type == "suck": + if not isinstance(object.model, object_type.Mesh): + half_x = 0 + half_y = 0 + half_z = 0 + + if isinstance(object.model, object_type.Box): + half_x = object.model.size_x / 2 + half_y = object.model.size_y / 2 + half_z = object.model.size_z / 2 + + elif isinstance(object.model, object_type.Cylinder): + half_y = half_x = object.model.radius + half_z = object.model.height / 2 + + elif isinstance(object.model, object_type.Sphere): + half_z = half_y = half_x = object.model.radius + + cx = object.pose.position.x + cy = object.pose.position.y + cz = object.pose.position.z + q = object.pose.orientation + + # TODO: dat moznost preferovanej rotacie ? + faces = [ + ((0.0, 0.0, half_z), (0.0, 0.0, 1.0), Orientation(1, 0, 0, 0))( # TOP + (-half_x, 0.0, 0.0), (-1.0, 0.0, 0.0), Orientation(0, 1, 0, 1) + ), # LEFT + ((half_x, 0.0, 0.0), (1.0, 0.0, 0.0), Orientation(0, -1, 0, 1)), # RIGHT + ((0.0, -half_y, 0.0), (0.0, -1.0, 0.0), Orientation(-1, 0, 0, 1)), # FRONT + ((0.0, half_y, 0.0), (0.0, 1.0, 0.0), Orientation(1, 0, 0, 1)), # BACK + ((0.0, 0.0, -half_z), (0.0, 0.0, -1.0), Orientation(0, 0, 1, 1)), + ] # BOTTOM + + for local_center, local_normal, local_effector_orientation in faces: + rcx, rcy, rcz = rotate_vector(q, local_center) + rnx, rny, rnz = rotate_vector(q, local_normal) + effector_orientation = rotate_effector(q, local_effector_orientation) + + face_x = cx + rcx + face_y = cy + rcy + face_z = cz + rcz + + grasp_pose = Pose( + Position( + face_x + rnx * grasp_offset, + face_y + rny * grasp_offset, + face_z + rnz * grasp_offset, + ), + effector_orientation, + ) + + pre_grasp_pose = Pose( + Position( + face_x + rnx * pre_grasp_offset, + face_y + rny * pre_grasp_offset, + face_z + rnz * pre_grasp_offset, + ), + effector_orientation, + ) + + grasp_poses.append((pre_grasp_pose, grasp_pose)) + else: # load for mesh pass + else: + pass # other type of effector return grasp_poses +def normalize_orientation(q: Orientation) -> Orientation: + norm = (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) ** 0.5 + if norm == 0.0: + raise UrGeneral("Invalid quaternion.") + return Orientation(q.x / norm, q.y / norm, q.z / norm, q.w / norm) + + +def compute_offset_pose_in_tool_axis(pose: Pose, distance: float) -> Pose: + q = normalize_orientation(pose.orientation) + dx, dy, dz = rotate_vector(q, (0.0, 0.0, -distance)) + + return Pose( + Position( + pose.position.x + dx, + pose.position.y + dy, + pose.position.z + dz, + ), + q, + ) + + +def compute_target_eef_pose(target_object_pose: Pose, attached_pose: Pose) -> Pose: + """Computes the target EEF pose so that the attached object ends up at target_object_pose. + target_object_pose: World pose where the object should be. + attached_pose: Pose of the object relative to the EEF (T_eef_obj). + """ + return tr.make_pose_abs(target_object_pose, attached_pose.inversed()) + + class MyNode(Node): def __init__( self, @@ -668,11 +765,14 @@ def apply_collision_objects(self, scene) -> None: for obj_id, obj in self.collision_objects.items(): state = obj.metadata.get("state") - if state == "LOST": + if state == GraspableState.LOST.value: + continue + + if state == GraspableState.HIDEN.value: continue # TODO: choose it by id - if state == "ATTACHED": + if state == GraspableState.ATTACHED.value: attached_pose_dict = obj.metadata.get("attached_pose") if attached_pose_dict is None: logger.warning(f"Attached object {obj_id} is missing attached_pose. Skipping.") @@ -779,66 +879,74 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]: raise UrGeneral("MoveIt is not initialized.") return self.moveitpy, self.ur_manipulator - def attach_object( - self, - effector_type: str, - grasp_pose: Pose, - object: CollisionSceneObject, - velocity: float, - payload: float, - safe: bool, - ) -> None: + def attach_object(self, object_id: str, effector_type: str, velocity: float, payload: float, safe: bool) -> None: + object = self.collision_objects[object_id] + object.metadata.pop("attached_pose", None) - if effector_type == "suck": - self.move_to_pose(grasp_pose, velocity, payload, safe) + grasp_options = generate_grasp_poses(object, effector_type) + for pre_grasp_pose, grasp_pose in grasp_options: - """ - self.suck(60) - time.sleep(1.0) + try: + self.move_to_pose(pre_grasp_pose, velocity, payload, safe) + object.metadata["state"] = GraspableState.HIDEN.value - vac = Vacuum.from_dict(self.vacuum()) - if vac.avg() < 20: - self.release() - raise UrGeneral(f"Failed to attach object {object.id}: vacuum not reached.") - """ - object.metadata["state"] = GraspableState.ATTACHED.value - attached_pose = tr.make_pose_rel(grasp_pose, object.pose) - object.metadata["attached_pose"] = attached_pose.to_dict() + """ + self.suck(60) + time.sleep(1.0) - # TODO: kozistencia s ur a scenou + spravnu orientaciu + vac = Vacuum.from_dict(self.vacuum()) + if vac.avg() < 20: + self.release() + raise UrGeneral(f"Failed to attach object {object.id}: vacuum not reached.") + """ + self.move_to_pose(grasp_pose, velocity, payload, safe) - else: - raise UrGeneral(f"Unsupported effector type: {effector_type}") + object.metadata["state"] = GraspableState.ATTACHED.value + attached_pose = tr.make_pose_rel(grasp_pose, object.pose) + object.metadata["attached_pose"] = attached_pose.to_dict() - return + except Exception: + continue - def detach_object(self, object: CollisionSceneObject, target_pose: Pose) -> None: - # self.release() + return - object.pose = target_pose - object.metadata["state"] = GraspableState.WORLD.value - object.metadata.pop("attached_pose", None) + raise UrGeneral(f"Unable to attach object {object_id}.") - def move_object_to_pose( - self, object_id: str, effector_type: str, target_pose: Pose, velocity: float, payload: float, safe: bool - ) -> None: + def detach_object(self, object_id: str, target_pose: Pose, velocity: float, payload: float, safe: bool) -> None: object = self.collision_objects[object_id] - grasp_options = generate_grasp_poses(object, effector_type) + attached_pose_dict = object.metadata.get("attached_pose") + if attached_pose_dict is None: + raise UrGeneral(f"Object {object_id} is not attached.") - for pre_grasp_pose, grasp_pose in grasp_options: + attached_pose = Pose.from_dict(attached_pose_dict) + target_eef_pose = compute_target_eef_pose(target_pose, attached_pose) + post_detach_pose = compute_offset_pose_in_tool_axis(target_eef_pose, 0.20) - try: - self.move_to_pose(pre_grasp_pose, velocity, payload, safe) - self.attach_object(effector_type, grasp_pose, object, velocity, payload, False) - except Exception: - continue + self.move_to_pose(target_eef_pose, velocity, payload, safe) - self.move_to_pose(target_pose, velocity, payload, safe) - self.detach_object(object, target_pose) - return + # self.release()S - raise UrGeneral(f"Unable to move object {object_id} to target pose.") + self.remove_object(object_id) + + object.metadata["state"] = GraspableState.HIDEN.value + object.metadata.pop("attached_pose", None) + self.update_collisions(self.collision_objects) + object.pose = target_pose + + self.move_to_pose(post_detach_pose, velocity, payload, safe) + + object.metadata["state"] = GraspableState.WORLD.value + self.update_collisions(self.collision_objects) + + def remove_object(self, object_id: str) -> None: + moveitpy, _ = self._get_moveit() + co = CollisionObject() + co.id = object_id + co.operation = CollisionObject.REMOVE + + with moveitpy.get_planning_scene_monitor().read_write() as scene: + scene.apply_collision_object(co) def ros_worker_main( @@ -905,11 +1013,18 @@ def ros_worker_main( elif op == "move_to_pose": pose = Pose.from_dict(kwargs["pose"]) result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"]) - elif op == "move_object_to_pose": - pose = Pose.from_dict(kwargs["pose"]) - result = runtime.move_object_to_pose( + elif op == "attach_object": + result = runtime.attach_object( kwargs["object_id"], kwargs["effector_type"], + kwargs["velocity"], + kwargs["payload"], + kwargs["safe"], + ) + elif op == "detach_object": + pose = Pose.from_dict(kwargs["pose"]) + result = runtime.detach_object( + kwargs["object_id"], pose, kwargs["velocity"], kwargs["payload"], diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index e4940b72..aaf0f9cb 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -284,9 +284,76 @@ def put_reserve_nearest_graspable() -> RespT: return jsonify(nearest_id) -@app.route("/graspable/move", methods=["PUT"]) +@app.route("/graspable/attach", methods=["PUT"]) @requires_started -def move_object() -> RespT: +def attach_object() -> RespT: + """Attaches a graspable object to the robot end-effector. + The robot first moves to a pre-attach pose and then to the attach pose. + --- + put: + tags: + - Graspable + summary: Attach a graspable object using pre-attach and attach poses. + requestBody: + required: true + content: + application/json: + schema: + type: object + required: + - object_id + - effector_type + properties: + object_id: + type: string + effector_type: + type: string + velocity: + type: number + format: float + default: 50.0 + payload: + type: number + format: float + default: 0.0 + safe: + type: boolean + default: true + responses: + 204: + description: Ok + 500: + description: "Error types: **General**." + content: + application/json: + schema: + $ref: WebApiError + """ + + body = humps.decamelize(request.json) + + object_id = body["object_id"] + effector_type = body["effector_type"] + velocity = float(body.get("velocity", 50.0)) / 100.0 + payload = float(body.get("payload", 0.0)) + safe = bool(body.get("safe", True)) + + assert globs.state + globs.state.worker.request( + "attach_object", + object_id=object_id, + effector_type=effector_type, + velocity=velocity, + payload=payload, + safe=safe, + ) + + return Response(status=204) + + +@app.route("/graspable/detach", methods=["PUT"]) +@requires_started +def detach_object() -> RespT: """Moves graspable object using robot. --- put: @@ -324,7 +391,6 @@ def move_object() -> RespT: body = humps.decamelize(request.json) object_id = body["object_id"] - effector_type = body["effector_type"] pose = Pose.from_dict(body["pose"]) velocity = float(body.get("velocity", 50.0)) / 100.0 payload = float(body.get("payload", 0.0)) @@ -332,9 +398,8 @@ def move_object() -> RespT: assert globs.state globs.state.worker.request( - "move_object_to_pose", + "detach_object", object_id=object_id, - effector_type=effector_type, pose=pose.to_dict(), velocity=velocity, payload=payload, From 6c69f60280a3ddfe6af5b20803ccebe06aa6403d Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Fri, 24 Apr 2026 01:28:16 +0200 Subject: [PATCH 06/11] feat: add suction grasp pick and place flow --- src/python/arcor2_object_types/abstract.py | 16 +- src/python/arcor2_scene/scripts/scene.py | 1 - src/python/arcor2_scene_data/scene_service.py | 8 - src/python/arcor2_ur/data/BUILD | 14 +- src/python/arcor2_ur/data/moveit.yaml | 4 +- src/python/arcor2_ur/data/urdf/BUILD | 9 +- .../ur5e/collision/suction_cup_1cup.stl | 4 +- src/python/arcor2_ur/data/urdf/ur5e.urdf | 36 +++ src/python/arcor2_ur/debug/BUILD | 26 ++ .../arcor2_ur/debug/custom_rsp.launch.py | 36 +++ .../arcor2_ur/debug/ur5e_custom.launch.py | 45 +++ src/python/arcor2_ur/object_types/ur5e.py | 44 +-- src/python/arcor2_ur/scripts/BUILD | 27 +- .../arcor2_ur/scripts/robot_publisher.py | 60 +++- src/python/arcor2_ur/scripts/ros_worker.py | 118 +++++--- src/python/arcor2_ur/scripts/ur.py | 271 +++++++++--------- src/python/arcor2_ur/tests/BUILD | 15 +- src/python/arcor2_ur/tests/conftest.py | 27 ++ .../arcor2_ur/tests/test_move_object.py | 32 +++ .../arcor2_ur/tests/test_overhead_obstacle.py | 24 +- .../arcor2_ur/tests/test_reserve_object.py | 49 ---- .../arcor2_ur/tests/test_side_obstacle.py | 25 +- .../arcor2_ur/tests/test_suck_effector.py | 41 +++ 23 files changed, 635 insertions(+), 297 deletions(-) create mode 100644 src/python/arcor2_ur/debug/BUILD create mode 100644 src/python/arcor2_ur/debug/custom_rsp.launch.py create mode 100644 src/python/arcor2_ur/debug/ur5e_custom.launch.py create mode 100644 src/python/arcor2_ur/tests/test_move_object.py delete mode 100644 src/python/arcor2_ur/tests/test_reserve_object.py create mode 100644 src/python/arcor2_ur/tests/test_suck_effector.py diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index a6e1665f..96c6b3a9 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -3,7 +3,6 @@ import inspect from dataclasses import dataclass from datetime import datetime, timezone -from enum import Enum from dataclasses_jsonschema import JsonSchemaMixin from PIL import Image @@ -185,6 +184,21 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None: set_enabled.__action__ = ActionMetadata() # type: ignore +class GraspPosition(StrEnum): + TOP = "TOP" + RIGHT = "RIGHT" + LEFT = "LEFT" + FRONT = "FRONT" + BACK = "BACK" + BOTTOM = "BOTTOM" + ALL = "ALL" + + +class EffectorType(StrEnum): + SUCK = "SUCK" + + +# TODO: zmenit na int class GraspableState(StrEnum): """Logical state of a graspable object in the scene. diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index e73d8429..8104c6da 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -5,7 +5,6 @@ import math import random import time -from typing import Any import humps import numpy as np diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py index bcc5b8c4..f9679aa8 100644 --- a/src/python/arcor2_scene_data/scene_service.py +++ b/src/python/arcor2_scene_data/scene_service.py @@ -158,13 +158,6 @@ def upsert_graspable( rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params) -# TODO: change endpoint /collisions/... ? -def reserve_nearest_graspable(position: Position, radius: float) -> str: - return rest.call( - rest.Method.PUT, f"{URL}/graspable/reserve-nearest", body=ReserveGraspable(position, radius), return_type=str - ) - - @handle(SceneServiceException, logger, message="Failed to delete the collision.") def delete_collision_id(collision_id: str) -> None: rest.call(rest.Method.DELETE, f"{URL}/collisions/{collision_id}") @@ -258,5 +251,4 @@ def world_pose(transform_id: str) -> Pose: upsert_transform.__name__, local_pose.__name__, world_pose.__name__, - reserve_nearest_graspable.__name__, ] diff --git a/src/python/arcor2_ur/data/BUILD b/src/python/arcor2_ur/data/BUILD index 1edd5c1b..7c8b8393 100644 --- a/src/python/arcor2_ur/data/BUILD +++ b/src/python/arcor2_ur/data/BUILD @@ -1 +1,13 @@ -resources(name="moveit.yaml", sources=["moveit.yaml"]) \ No newline at end of file +resources( + name="moveit_yaml", + sources=["moveit.yaml"], +) + +resources( + name="urdf_assets", + sources=[ + "urdf/ur5e.urdf", + "urdf/meshes/ur5e/collision/*.stl", + "urdf/meshes/ur5e/visual/*.dae", + ], +) \ No newline at end of file diff --git a/src/python/arcor2_ur/data/moveit.yaml b/src/python/arcor2_ur/data/moveit.yaml index f7ac8d26..08e944c4 100644 --- a/src/python/arcor2_ur/data/moveit.yaml +++ b/src/python/arcor2_ur/data/moveit.yaml @@ -11,12 +11,12 @@ planning_pipelines: pipeline_names: ["ompl"] plan_request_params: - planning_attempts: 2 + planning_attempts: 5 planning_pipeline: ompl planner_id: RRTConnectkConfigDefault max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 - planning_time: 5.0 + planning_time: 10.0 goal_joint_tolerance: 0.001 goal_position_tolerance: 0.005 goal_orientation_tolerance: 0.01 diff --git a/src/python/arcor2_ur/data/urdf/BUILD b/src/python/arcor2_ur/data/urdf/BUILD index 73b4efc3..ea7c4948 100644 --- a/src/python/arcor2_ur/data/urdf/BUILD +++ b/src/python/arcor2_ur/data/urdf/BUILD @@ -1,8 +1,7 @@ -resources( - name="ur5e", +files( + name="urdf", sources=[ "ur5e.urdf", - "meshes/ur5e/collision/*.stl", - "meshes/ur5e/visual/*.dae", + "meshes/**", ], -) +) \ No newline at end of file diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl index 67b71ffa..3c94a3d0 100644 --- a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:10686a45842b55d7ce5dc6d3dbf1e51b235a1722e4697e4cec2ccf22e65bbfc4 -size 250034 +oid sha256:2faa852002f4d06edc18faf5f9de3753e5e46d9349f75acea44812ac25b28a2c +size 249334 diff --git a/src/python/arcor2_ur/data/urdf/ur5e.urdf b/src/python/arcor2_ur/data/urdf/ur5e.urdf index 77750df7..ad38555b 100644 --- a/src/python/arcor2_ur/data/urdf/ur5e.urdf +++ b/src/python/arcor2_ur/data/urdf/ur5e.urdf @@ -288,4 +288,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/python/arcor2_ur/debug/BUILD b/src/python/arcor2_ur/debug/BUILD new file mode 100644 index 00000000..3cc4b908 --- /dev/null +++ b/src/python/arcor2_ur/debug/BUILD @@ -0,0 +1,26 @@ +python_sources( + name="lib", + dependencies=[ + "src/python/arcor2_ur/data:urdf_assets", + ], +) + +pex_binary( + name="debug", + entry_point="arcor2_ur.debug.debug", + dependencies=[ + ":lib", + "src/python/arcor2_ur/scripts:ur", + "src/python/arcor2_ur/scripts:robot_publisher", + ], +) + +pex_binary( + name="box_in_path_debug", + entry_point="arcor2_ur.debug.box_in_path_debug", + dependencies=[ + ":lib", + "src/python/arcor2_ur/scripts:ur", + "src/python/arcor2_ur/scripts:robot_publisher", + ], +) \ No newline at end of file diff --git a/src/python/arcor2_ur/debug/custom_rsp.launch.py b/src/python/arcor2_ur/debug/custom_rsp.launch.py new file mode 100644 index 00000000..91c6fa25 --- /dev/null +++ b/src/python/arcor2_ur/debug/custom_rsp.launch.py @@ -0,0 +1,36 @@ +# noqa: TAE001 +# mypy: ignore-errors +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + package_root = Path(__file__).resolve().parents[1] + urdf_path = package_root / "data" / "urdf" / "ur5e.urdf" + meshes_dir = package_root / "data" / "urdf" / "meshes" + + text = urdf_path.read_text(encoding="utf-8") + text = text.replace( + "package://meshes/", + f"file://{meshes_dir.as_posix()}/", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("ur_type", default_value="ur5e"), + DeclareLaunchArgument("robot_ip", default_value="xyz"), + DeclareLaunchArgument("use_mock_hardware", default_value="true"), + DeclareLaunchArgument("mock_sensor_commands", default_value="false"), + DeclareLaunchArgument("headless_mode", default_value="false"), + DeclareLaunchArgument("kinematics_parameters_file", default_value=""), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[{"robot_description": text}], + ), + ] + ) diff --git a/src/python/arcor2_ur/debug/ur5e_custom.launch.py b/src/python/arcor2_ur/debug/ur5e_custom.launch.py new file mode 100644 index 00000000..a05f9383 --- /dev/null +++ b/src/python/arcor2_ur/debug/ur5e_custom.launch.py @@ -0,0 +1,45 @@ +# noqa: TAE001 +# mypy: ignore-errors +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + declared_arguments = [ + DeclareLaunchArgument("robot_ip"), + DeclareLaunchArgument("use_mock_hardware", default_value="false"), + DeclareLaunchArgument("mock_sensor_commands", default_value="false"), + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + ), + DeclareLaunchArgument("activate_joint_controller", default_value="true"), + ] + + robot_ip = LaunchConfiguration("robot_ip") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + + ur_control = "/opt/ros/jazzy/share/ur_robot_driver/launch/ur_control.launch.py" + custom_rsp = str(Path(__file__).resolve().parent / "custom_rsp.launch.py") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(ur_control), + launch_arguments={ + "ur_type": "ur5e", + "robot_ip": robot_ip, + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "initial_joint_controller": initial_joint_controller, + "activate_joint_controller": activate_joint_controller, + "description_launchfile": custom_rsp, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index 229ad64a..2d7cfb9b 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -1,12 +1,12 @@ import time -from dataclasses import dataclass +from dataclasses import dataclass, field from typing import cast from dataclasses_jsonschema import JsonSchemaMixin from arcor2.data.common import ActionMetadata, Joint, Pose, Position, StrEnum from arcor2.data.robot import InverseKinematicsRequest -from arcor2_object_types.abstract import Robot, Settings +from arcor2_object_types.abstract import EffectorType, GraspPosition, Robot, Settings from arcor2_web import rest @@ -25,9 +25,12 @@ def avg(self) -> float: @dataclass -class AttachObject(JsonSchemaMixin): - object_id: str - effector_type: str +class PickUpObject(JsonSchemaMixin): + position: Position + radius: float + effector_type: EffectorType = EffectorType.SUCK + grasp_positions: list[GraspPosition] = field(default_factory=lambda: [GraspPosition.ALL]) + object_type_name: None | str = None velocity: float = 50.0 payload: float = 0.0 safe: bool = True @@ -35,7 +38,6 @@ class AttachObject(JsonSchemaMixin): @dataclass class DetachObject(JsonSchemaMixin): - object_id: str pose: Pose velocity: float = 50.0 payload: float = 0.0 @@ -91,7 +93,7 @@ def grippers(self) -> set[str]: return set() def suctions(self) -> set[str]: - return set("default") + return {"default"} def get_end_effector_pose(self, end_effector_id: str) -> Pose: return rest.call(rest.Method.GET, f"{self.settings.url}/eef/pose", return_type=Pose) @@ -131,33 +133,42 @@ def move( params={"velocity": speed, "payload": payload, "safe": safe}, ) - def attach_object( + def pick_up_object( self, - object_id: str, - effector_type: str, + position: Position, + radius: float, + effector_type: EffectorType = EffectorType.SUCK, + grasp_positions: None | list[GraspPosition] = None, + object_type_name: None | str = None, speed: float = 50.0, safe: bool = True, payload: float = 0.0, ) -> None: + assert radius >= 0.0 assert 0.0 <= speed <= 100.0 assert 0.0 <= payload <= 5.0 + if grasp_positions is None: + grasp_positions = [GraspPosition.ALL] + with self._move_lock: rest.call( rest.Method.PUT, - f"{self.settings.url}/graspable/attach", - body=AttachObject( - object_id=object_id, + f"{self.settings.url}/graspable/pick_up_object", + body=PickUpObject( + position=position, + radius=radius, effector_type=effector_type, + grasp_positions=grasp_positions, + object_type_name=object_type_name, velocity=speed, payload=payload, safe=safe, ), ) - def detach_object( + def place_object( self, - object_id: str, pose: Pose, speed: float = 50.0, safe: bool = True, @@ -169,9 +180,8 @@ def detach_object( with self._move_lock: rest.call( rest.Method.PUT, - f"{self.settings.url}/graspable/detach", + f"{self.settings.url}/graspable/place_object", body=DetachObject( - object_id=object_id, pose=pose, velocity=speed, payload=payload, diff --git a/src/python/arcor2_ur/scripts/BUILD b/src/python/arcor2_ur/scripts/BUILD index f1f8f16f..a909b292 100644 --- a/src/python/arcor2_ur/scripts/BUILD +++ b/src/python/arcor2_ur/scripts/BUILD @@ -1,14 +1,31 @@ -python_sources() +python_sources( + dependencies=[ + "src/python/arcor2_ur/data:moveit_yaml", + "src/python/arcor2_ur/data:urdf_assets", + ], +) arcor2_pex_binary( name="ur", - dependencies=["3rdparty#lark", "src/python/arcor2_ur/data:moveit.yaml"], + dependencies=[ + "3rdparty#lark", + "src/python/arcor2_ur/data:moveit_yaml", + "src/python/arcor2_ur/data:urdf_assets", + ], ) + arcor2_pex_binary( - name="upload_objects", dependencies=["src/python/arcor2_ur/data/urdf:ur5e"] + name="upload_objects", + dependencies=[ + "src/python/arcor2_ur/data:urdf_assets", + ], ) arcor2_pex_binary( name="robot_publisher", - dependencies=["3rdparty#PyYAML", "3rdparty#numpy"], -) + dependencies=[ + "3rdparty#PyYAML", + "3rdparty#numpy", + "src/python/arcor2_ur/data:urdf_assets", + ], +) \ No newline at end of file diff --git a/src/python/arcor2_ur/scripts/robot_publisher.py b/src/python/arcor2_ur/scripts/robot_publisher.py index 564c61d5..d02dde08 100644 --- a/src/python/arcor2_ur/scripts/robot_publisher.py +++ b/src/python/arcor2_ur/scripts/robot_publisher.py @@ -1,34 +1,84 @@ +import os +import subprocess as sp +from importlib import resources + import rclpy # pants: no-infer-dep from rclpy.executors import ExternalShutdownException # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep -from std_msgs.msg import Bool # pants: no-infer-dep +from std_msgs.msg import Bool, String # pants: no-infer-dep from ur_dashboard_msgs.msg import RobotMode # pants: no-infer-dep from arcor2_ur.topics import ROBOT_MODE_TOPIC, ROBOT_PROGRAM_RUNNING_TOPIC +def load_robot_description() -> str: + urdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.urdf") + meshes_res = resources.files("arcor2_ur").joinpath("data/urdf/meshes") + + text = urdf_res.read_text(encoding="utf-8") + + with resources.as_file(meshes_res) as meshes_dir: + text = text.replace( + "package://meshes/", + f"file://{meshes_dir.as_posix()}/", + ) + + return text + + +def load_robot_description_semantic() -> str: + ur_type = os.getenv("ARCOR2_UR_TYPE", "ur5e") + cmd = [ + "bash", + "-lc", + ( + "source /opt/ros/jazzy/setup.bash >/dev/null && " + f"xacro /opt/ros/jazzy/share/ur_moveit_config/srdf/ur.srdf.xacro name:={ur_type}" + ), + ] + return sp.check_output(cmd, text=True).strip() + + class PublisherNode(Node): def __init__(self) -> None: super().__init__("minimal_publisher") qos = QoSProfile( - depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE + depth=1, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, ) self.robot_program_running_pub = self.create_publisher(Bool, ROBOT_PROGRAM_RUNNING_TOPIC, qos) self.robot_mode_pub = self.create_publisher(RobotMode, ROBOT_MODE_TOPIC, qos) + self.robot_description_pub = self.create_publisher(String, "/robot_description", qos) + self.robot_description_semantic_pub = self.create_publisher(String, "/robot_description_semantic", qos) + + self.robot_description_msg = String() + self.robot_description_msg.data = load_robot_description() + + self.robot_description_semantic_msg = String() + self.robot_description_semantic_msg.data = load_robot_description_semantic() + + self.timer = self.create_timer(1.0, self._republish) + + def _republish(self) -> None: + self.robot_program_running_pub.publish(Bool(data=True)) + self.robot_mode_pub.publish(RobotMode(mode=RobotMode.RUNNING)) + self.robot_description_pub.publish(self.robot_description_msg) + self.robot_description_semantic_pub.publish(self.robot_description_semantic_msg) def main() -> None: rclpy.init() publisher_node = PublisherNode() - publisher_node.robot_program_running_pub.publish(Bool(data=True)) - publisher_node.robot_mode_pub.publish(RobotMode(mode=RobotMode.RUNNING)) + + publisher_node._republish() + try: rclpy.spin(publisher_node) except ExternalShutdownException: - # Happens when the process is terminated from the outside; exit cleanly. pass finally: publisher_node.destroy_node() diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index e821959f..dc7f5acc 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -1,5 +1,4 @@ import importlib -import math import multiprocessing import threading import time @@ -29,7 +28,7 @@ from arcor2.data.common import Joint, Orientation, Pose, Position from arcor2.data.robot import InverseKinematicsRequest from arcor2.logging import get_logger -from arcor2_object_types.abstract import GraspableState +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_ur import topics from arcor2_ur.common import CollisionSceneObject from arcor2_ur.exceptions import UrGeneral @@ -156,17 +155,24 @@ def rotate_effector(a: Orientation, b: Orientation) -> Orientation: ) -def generate_grasp_poses(object: CollisionSceneObject, effector_type: str) -> list[tuple[Pose, Pose]]: +def generate_grasp_poses( + object: CollisionSceneObject, + effector_type: EffectorType, + grasp_positions: list[str] | None, +) -> list[tuple[Pose, Pose]]: + if grasp_positions is None: + grasp_positions = [GraspPosition.ALL.value] + grasp_poses: list[tuple[Pose, Pose]] = [] pre_grasp_offset = 0.20 - grasp_offset = 0.01 # TODO: -0.01 ? + grasp_offset = -0.01 # TODO: -0.01 ? - if effector_type == "suck": + if effector_type == EffectorType.SUCK: if not isinstance(object.model, object_type.Mesh): - half_x = 0 - half_y = 0 - half_z = 0 + half_x = 0.0 + half_y = 0.0 + half_z = 0.0 if isinstance(object.model, object_type.Box): half_x = object.model.size_x / 2 @@ -185,16 +191,34 @@ def generate_grasp_poses(object: CollisionSceneObject, effector_type: str) -> li cz = object.pose.position.z q = object.pose.orientation - # TODO: dat moznost preferovanej rotacie ? - faces = [ - ((0.0, 0.0, half_z), (0.0, 0.0, 1.0), Orientation(1, 0, 0, 0))( # TOP - (-half_x, 0.0, 0.0), (-1.0, 0.0, 0.0), Orientation(0, 1, 0, 1) - ), # LEFT - ((half_x, 0.0, 0.0), (1.0, 0.0, 0.0), Orientation(0, -1, 0, 1)), # RIGHT - ((0.0, -half_y, 0.0), (0.0, -1.0, 0.0), Orientation(-1, 0, 0, 1)), # FRONT - ((0.0, half_y, 0.0), (0.0, 1.0, 0.0), Orientation(1, 0, 0, 1)), # BACK - ((0.0, 0.0, -half_z), (0.0, 0.0, -1.0), Orientation(0, 0, 1, 1)), - ] # BOTTOM + faces = [] + if GraspPosition.ALL in grasp_positions: + faces = [ + ((0.0, 0.0, half_z), (0.0, 0.0, 1.0), Orientation(1, 0, 0, 0)), # TOP + ((-half_x, 0.0, 0.0), (-1.0, 0.0, 0.0), Orientation(0, 1, 0, 1)), # LEFT + ((half_x, 0.0, 0.0), (1.0, 0.0, 0.0), Orientation(0, -1, 0, 1)), # RIGHT + ((0.0, -half_y, 0.0), (0.0, -1.0, 0.0), Orientation(-1, 0, 0, 1)), # FRONT + ((0.0, half_y, 0.0), (0.0, 1.0, 0.0), Orientation(1, 0, 0, 1)), # BACK + ((0.0, 0.0, -half_z), (0.0, 0.0, -1.0), Orientation(0, 0, 1, 1)), # BOTTOM + ] + else: + if GraspPosition.TOP in grasp_positions: + faces.append(((0.0, 0.0, half_z), (0.0, 0.0, 1.0), Orientation(1, 0, 0, 0))) + + if GraspPosition.LEFT in grasp_positions: + faces.append(((-half_x, 0.0, 0.0), (-1.0, 0.0, 0.0), Orientation(0, 1, 0, 1))) + + if GraspPosition.RIGHT in grasp_positions: + faces.append(((half_x, 0.0, 0.0), (1.0, 0.0, 0.0), Orientation(0, -1, 0, 1))) + + if GraspPosition.FRONT in grasp_positions: + faces.append(((0.0, -half_y, 0.0), (0.0, -1.0, 0.0), Orientation(-1, 0, 0, 1))) + + if GraspPosition.BACK in grasp_positions: + faces.append(((0.0, half_y, 0.0), (0.0, 1.0, 0.0), Orientation(1, 0, 0, 1))) + + if GraspPosition.BOTTOM in grasp_positions: + faces.append(((0.0, 0.0, -half_z), (0.0, 0.0, -1.0), Orientation(0, 0, 0, 1))) for local_center, local_normal, local_effector_orientation in faces: rcx, rcy, rcz = rotate_vector(q, local_center) @@ -879,28 +903,44 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]: raise UrGeneral("MoveIt is not initialized.") return self.moveitpy, self.ur_manipulator - def attach_object(self, object_id: str, effector_type: str, velocity: float, payload: float, safe: bool) -> None: + def get_attached_object_id(self) -> str | None: + for obj_id, obj in self.collision_objects.items(): + if obj.metadata.get("state") == GraspableState.ATTACHED.value: + return obj_id + return None + + def attach_object( + self, + object_id: str, + effector_type: EffectorType, + grasp_positions: list[str] | None, + velocity: float, + payload: float, + safe: bool, + ) -> None: object = self.collision_objects[object_id] object.metadata.pop("attached_pose", None) - grasp_options = generate_grasp_poses(object, effector_type) + grasp_options = generate_grasp_poses(object, effector_type, grasp_positions) for pre_grasp_pose, grasp_pose in grasp_options: try: self.move_to_pose(pre_grasp_pose, velocity, payload, safe) object.metadata["state"] = GraspableState.HIDEN.value - """ - self.suck(60) - time.sleep(1.0) + if self.tool: # Skip vacuum activation/check when no physical tool is configured + self.suck(60) + time.sleep(1.0) - vac = Vacuum.from_dict(self.vacuum()) - if vac.avg() < 20: - self.release() - raise UrGeneral(f"Failed to attach object {object.id}: vacuum not reached.") - """ self.move_to_pose(grasp_pose, velocity, payload, safe) + if self.tool: + vac = Vacuum.from_dict(self.vacuum()) + if vac.avg() < 20: + self.release() + raise UrGeneral(f"Failed to attach object {object_id}: vacuum not reached.") + + # TODO: send info to ur object.metadata["state"] = GraspableState.ATTACHED.value attached_pose = tr.make_pose_rel(grasp_pose, object.pose) object.metadata["attached_pose"] = attached_pose.to_dict() @@ -912,7 +952,11 @@ def attach_object(self, object_id: str, effector_type: str, velocity: float, pay raise UrGeneral(f"Unable to attach object {object_id}.") - def detach_object(self, object_id: str, target_pose: Pose, velocity: float, payload: float, safe: bool) -> None: + def detach_object(self, target_pose: Pose, velocity: float, payload: float, safe: bool) -> None: + object_id = self.get_attached_object_id() + if object_id is None: + raise UrGeneral("No object to detach.") + object = self.collision_objects[object_id] attached_pose_dict = object.metadata.get("attached_pose") @@ -925,7 +969,7 @@ def detach_object(self, object_id: str, target_pose: Pose, velocity: float, payl self.move_to_pose(target_eef_pose, velocity, payload, safe) - # self.release()S + self.release() self.remove_object(object_id) @@ -1013,23 +1057,25 @@ def ros_worker_main( elif op == "move_to_pose": pose = Pose.from_dict(kwargs["pose"]) result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"]) - elif op == "attach_object": - result = runtime.attach_object( + elif op == "pick_up_object": + runtime.attach_object( kwargs["object_id"], - kwargs["effector_type"], + EffectorType(kwargs["effector_type"]), + [GraspPosition(gp) for gp in kwargs["grasp_positions"]], kwargs["velocity"], kwargs["payload"], kwargs["safe"], ) - elif op == "detach_object": + result = {} + elif op == "place_object": pose = Pose.from_dict(kwargs["pose"]) - result = runtime.detach_object( - kwargs["object_id"], + runtime.detach_object( pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"], ) + result = {} elif op == "get_joints": result = runtime.get_joints() elif op == "ik": diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index aaf0f9cb..cfb9f758 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -5,8 +5,7 @@ import os from dataclasses import dataclass, field from functools import wraps -from math import dist -from typing import Any +from importlib import resources import humps from ament_index_python.packages import get_package_share_directory # pants: no-infer-dep @@ -19,8 +18,8 @@ from arcor2.data.robot import InverseKinematicsRequest from arcor2.helpers import port_from_url from arcor2.logging import get_logger -from arcor2_object_types.abstract import GraspableState -from arcor2_ur import get_data, version +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_ur import version from arcor2_ur.common import CollisionSceneObject, parse_collision_body from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum @@ -31,7 +30,7 @@ URL = os.getenv("ARCOR2_UR_URL", "http://localhost:5012") BASE_LINK = os.getenv("ARCOR2_UR_BASE_LINK", "base_link") -TOOL_LINK = os.getenv("ARCOR2_UR_TOOL_LINK", "tool0") +TOOL_LINK = os.getenv("ARCOR2_UR_TOOL_LINK", "suction_tcp") UR_TYPE = os.getenv("ARCOR2_UR_TYPE", "ur5e") PLANNING_GROUP_NAME = os.getenv("ARCOR2_UR_PLANNING_GROUP_NAME", "ur_manipulator") ROBOT_IP = os.getenv("ARCOR2_UR_ROBOT_IP", "") @@ -58,25 +57,43 @@ class Globs: globs: Globs = Globs() app = create_app(__name__) + # this is normally specified in a launch file -moveit_config = ( - MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config") - .robot_description( - os.path.join(get_package_share_directory("ur_description"), "urdf", "ur.urdf.xacro"), - {"name": "ur", "ur_type": UR_TYPE}, - ) - .robot_description_semantic( - os.path.join(get_package_share_directory("ur_moveit_config"), "srdf", "ur.srdf.xacro"), {"name": UR_TYPE} - ) - .trajectory_execution( - os.path.join(get_package_share_directory("ur_moveit_config"), "config", "moveit_controllers.yaml") - ) - .robot_description_kinematics( - os.path.join(get_package_share_directory("ur_moveit_config"), "config", "kinematics.yaml") - ) - .moveit_cpp(file_path=get_data("moveit.yaml")) - .to_moveit_configs() -).to_dict() +def load_custom_urdf() -> str: + urdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.urdf") + meshes_res = resources.files("arcor2_ur").joinpath("data/urdf/meshes") + + text = urdf_res.read_text(encoding="utf-8") + + with resources.as_file(meshes_res) as meshes_dir: + text = text.replace( + "package://meshes/", + f"file://{meshes_dir.as_posix()}/", + ) + + return text + + +moveit_yaml_res = resources.files("arcor2_ur").joinpath("data/moveit.yaml") + +with resources.as_file(moveit_yaml_res) as moveit_yaml_path: + moveit_config = ( + MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config") + .robot_description_semantic( + os.path.join(get_package_share_directory("ur_moveit_config"), "srdf", "ur.srdf.xacro"), + {"name": UR_TYPE}, + ) + .trajectory_execution( + os.path.join(get_package_share_directory("ur_moveit_config"), "config", "moveit_controllers.yaml") + ) + .robot_description_kinematics( + os.path.join(get_package_share_directory("ur_moveit_config"), "config", "kinematics.yaml") + ) + .moveit_cpp(file_path=str(moveit_yaml_path)) + .to_moveit_configs() + ).to_dict() + +moveit_config["robot_description"] = load_custom_urdf() def started() -> bool: @@ -196,104 +213,15 @@ def get_started() -> RespT: return jsonify(started()) -@app.route("/graspable/reserve-nearest", methods=["PUT"]) -def put_reserve_nearest_graspable() -> RespT: - """Reserve nearest graspable object in WORLD state. - --- - put: - tags: - - Graspable - description: Find the nearest graspable object in WORLD state within the given radius, mark it as RESERVED, and return its ID. If no such object is found, returns null. - requestBody: - content: - application/json: - schema: - type: object - required: - - position - - radius - properties: - position: - $ref: Position - radius: - type: number - format: float - responses: - 200: - description: ID of the reserved graspable object, or null if no matching object was found. - content: - application/json: - schema: - oneOf: - - type: string - - type: "null" - 500: - description: "Error types: **General**, **UrGeneral**." - content: - application/json: - schema: - $ref: WebApiError - """ - if not isinstance(request.json, dict): - raise UrGeneral("Body should be a JSON dict containing position and radius.") - - body = humps.decamelize(request.json) - - try: - position = common.Position.from_dict(body["position"]) - radius = float(body["radius"]) - except KeyError as exc: - raise UrGeneral(f"Missing key: {exc.args[0]}") - - if radius < 0.0: - raise UrGeneral("Radius has to be >= 0.") - - nearest_id: str | None = None - nearest_distance: float | None = None - - for obj_id, obj in globs.collision_objects.items(): - - # only graspable objects - if obj.metadata.get("object_type") != "graspable": - continue - - # only WORLD objects - if obj.metadata.get("state") != GraspableState.WORLD.value: - continue - - current_distance = position.distance(obj.pose.position) - - if current_distance >= radius: - continue - - if nearest_distance is None or current_distance < nearest_distance: - nearest_id = obj_id - nearest_distance = current_distance - - if nearest_id is None: - return jsonify("") - - # change object state to RESERVED - globs.collision_objects[nearest_id].metadata["state"] = GraspableState.RESERVED.value - - # refresh worker - if started(): - assert globs.state - globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) - - return jsonify(nearest_id) - - -@app.route("/graspable/attach", methods=["PUT"]) +@app.route("/graspable/pick_up_object", methods=["PUT"]) @requires_started -def attach_object() -> RespT: - """Attaches a graspable object to the robot end-effector. - The robot first moves to a pre-attach pose and then to the attach pose. +def pick_up_object() -> RespT: + """Find nearest graspable object and attach it. --- put: tags: - Graspable - summary: Attach a graspable object using pre-attach and attach poses. + summary: Find nearest graspable object in radius and attach it. requestBody: required: true content: @@ -301,13 +229,24 @@ def attach_object() -> RespT: schema: type: object required: - - object_id - - effector_type + - position + - radius properties: - object_id: - type: string + position: + $ref: Position + radius: + type: number + format: float effector_type: type: string + default: suck + grasp_positions: + type: array + items: + type: string + model3d_type: + type: string + default: none velocity: type: number format: float @@ -323,37 +262,98 @@ def attach_object() -> RespT: 204: description: Ok 500: - description: "Error types: **General**." + description: "Error types: **General**, **UrGeneral**." content: application/json: schema: $ref: WebApiError """ + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict.") body = humps.decamelize(request.json) - object_id = body["object_id"] - effector_type = body["effector_type"] + try: + position = common.Position.from_dict(body["position"]) + radius = float(body["radius"]) + except KeyError as exc: + raise UrGeneral(f"Missing key: {exc.args[0]}") + + if radius < 0.0: + raise UrGeneral("Radius has to be >= 0.") + + effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK.value)) + grasp_positions = body.get( + "grasp_positions", + [ + GraspPosition.TOP.value, + GraspPosition.RIGHT.value, + GraspPosition.LEFT.value, + GraspPosition.FRONT.value, + GraspPosition.BACK.value, + GraspPosition.BOTTOM.value, + GraspPosition.ALL, + ], + ) + + object_type_cls = getattr(object_type, body["object_type_name"]) if body.get("object_type_name") else None + velocity = float(body.get("velocity", 50.0)) / 100.0 payload = float(body.get("payload", 0.0)) safe = bool(body.get("safe", True)) + nearest_id: str | None = None + nearest_distance: float | None = None + + for obj_id, obj in globs.collision_objects.items(): + if obj.metadata.get("object_type") != "graspable": + continue + + if obj.metadata.get("state") != GraspableState.WORLD.value: + continue + + if object_type_cls is not None and not isinstance(obj.model, object_type_cls): + continue + + current_distance = position.distance(obj.pose.position) + if current_distance > radius: + continue + + if nearest_distance is None or current_distance < nearest_distance: + nearest_id = obj_id + nearest_distance = current_distance + + if nearest_id is None: + raise UrGeneral("No graspable object found for given position, radius and type.") + + selected = globs.collision_objects[nearest_id] + previous_state = selected.metadata.get("state") + selected.metadata["state"] = GraspableState.RESERVED.value + assert globs.state - globs.state.worker.request( - "attach_object", - object_id=object_id, - effector_type=effector_type, - velocity=velocity, - payload=payload, - safe=safe, - ) + + try: + globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) + globs.state.worker.request( + "pick_up_object", + object_id=nearest_id, + effector_type=effector_type, + grasp_positions=grasp_positions, + velocity=velocity, + payload=payload, + safe=safe, + ) + except Exception: + selected.metadata["state"] = previous_state + globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) + raise return Response(status=204) -@app.route("/graspable/detach", methods=["PUT"]) +@app.route("/graspable/place_object", methods=["PUT"]) @requires_started -def detach_object() -> RespT: +def place_object() -> RespT: """Moves graspable object using robot. --- put: @@ -367,7 +367,6 @@ def detach_object() -> RespT: schema: type: object required: - - object_id - effector_type - pose properties: @@ -390,7 +389,6 @@ def detach_object() -> RespT: body = humps.decamelize(request.json) - object_id = body["object_id"] pose = Pose.from_dict(body["pose"]) velocity = float(body.get("velocity", 50.0)) / 100.0 payload = float(body.get("payload", 0.0)) @@ -398,8 +396,7 @@ def detach_object() -> RespT: assert globs.state globs.state.worker.request( - "detach_object", - object_id=object_id, + "place_object", pose=pose.to_dict(), velocity=velocity, payload=payload, diff --git a/src/python/arcor2_ur/tests/BUILD b/src/python/arcor2_ur/tests/BUILD index 83ad122a..838bf73d 100644 --- a/src/python/arcor2_ur/tests/BUILD +++ b/src/python/arcor2_ur/tests/BUILD @@ -1,8 +1,17 @@ python_tests( - dependencies=["3rdparty#PyYAML"], + name="tests", + sources=["test_*.py"], + dependencies=[ + "src/python/arcor2_ur/data/urdf:urdf", + "src/python/arcor2_ur/data:moveit_yaml", + ], runtime_package_dependencies=[ - "src/python/arcor2_ur/scripts:ur", "src/python/arcor2_ur/scripts:robot_publisher", + "src/python/arcor2_ur/scripts:ur", + "src/python/arcor2_ur/scripts:robot_publisher", ], ) -python_test_utils(name="test_utils") +python_test_utils( + name="test_utils", + sources=["conftest.py"], +) \ No newline at end of file diff --git a/src/python/arcor2_ur/tests/conftest.py b/src/python/arcor2_ur/tests/conftest.py index 18c45c02..8a6f9a3b 100644 --- a/src/python/arcor2_ur/tests/conftest.py +++ b/src/python/arcor2_ur/tests/conftest.py @@ -4,6 +4,7 @@ import signal import subprocess as sp import time +from pathlib import Path from typing import Iterator, NamedTuple import pytest @@ -100,6 +101,32 @@ def start_processes(request) -> Iterator[Urls]: log_proc_output(processes[-1].communicate()) raise Exception("Launch died...") + # kill default robot_state_publisher + sp.run("pkill -f robot_state_publisher", shell=True) + + # start custom robot_state_publisher with modified URDF + urdf_path = Path(__file__).resolve().parents[1] / "data" / "urdf" / "ur5e.urdf" + + custom_rsp = sp.Popen( + [ + "ros2", + "run", + "robot_state_publisher", + "robot_state_publisher", + "--ros-args", + "-p", + f"robot_description:={urdf_path.read_text()}", + ], + env=ros_launch_env, + stdout=sp.PIPE, + stderr=sp.STDOUT, + start_new_session=True, + ) + + processes.append(custom_rsp) + + time.sleep(2) + robot_url = f"http://0.0.0.0:{find_free_port()}" my_env["ARCOR2_UR_URL"] = robot_url my_env["ARCOR2_UR_INTERACT_WITH_DASHBOARD"] = "false" diff --git a/src/python/arcor2_ur/tests/test_move_object.py b/src/python/arcor2_ur/tests/test_move_object.py new file mode 100644 index 00000000..3d8ef39d --- /dev/null +++ b/src/python/arcor2_ur/tests/test_move_object.py @@ -0,0 +1,32 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +# TODO: add asserts +@pytest.mark.timeout(321) +def test_move_object(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + box = Box("Box1", 0.1, 0.1, 0.2) + scene_service.upsert_graspable(box, Pose(Position(0, 0.5, 0.1), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object(Position(0, 0.5, 0.1), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + + ot.place_object(Pose(Position(0, -0.5, 0.1), Orientation(0, 0, 0, 1))) + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_overhead_obstacle.py index 5c8790b3..f0e1ca28 100644 --- a/src/python/arcor2_ur/tests/test_overhead_obstacle.py +++ b/src/python/arcor2_ur/tests/test_overhead_obstacle.py @@ -18,12 +18,12 @@ def test_box(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 + X = 0.5 Y = 0.0 - Z = 0.2 - X = 0.9 + Z = 0.3 - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) box = Box("Box1", 0.2, 0.2, 0.2) @@ -64,12 +64,12 @@ def test_cylinder(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 + X = 0.5 Y = 0.0 - Z = 0.2 - X = 0.9 + Z = 0.3 - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) cyl = Cylinder("Cyl1", 0.2, 0.2) @@ -110,12 +110,12 @@ def test_sphere(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 + X = 0.5 Y = 0.0 - Z = 0.2 - X = 0.9 + Z = 0.3 - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) sphere = Sphere("Sphere1", 0.09) diff --git a/src/python/arcor2_ur/tests/test_reserve_object.py b/src/python/arcor2_ur/tests/test_reserve_object.py deleted file mode 100644 index d6966d23..00000000 --- a/src/python/arcor2_ur/tests/test_reserve_object.py +++ /dev/null @@ -1,49 +0,0 @@ -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Cylinder -from arcor2_object_types.abstract import GraspableSource, GraspableState -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -@pytest.mark.timeout(321) -def test_reserve_object(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - - cyl1 = Cylinder("Cyl1", 0.1, 0.95) - scene_service.upsert_graspable( - cyl1, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), state=GraspableState.WORLD - ) - assert cyl1.id in scene_service.collision_ids() - time.sleep(1) - - cyl2 = Cylinder("Cyl2", 0.1, 0.95) - scene_service.upsert_collision(cyl2, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl2.id in scene_service.collision_ids() - time.sleep(1) - - cyl3 = Cylinder("Cyl3", 0.1, 0.95) - scene_service.upsert_graspable( - cyl3, - Pose(Position(0.0, 0.0, 0.0), Orientation(0, 0, 0, 1)), - state=GraspableState.WORLD, - source=GraspableSource.OTHER, - ) - assert cyl3.id in scene_service.collision_ids() - time.sleep(1) - - assert cyl1.id == scene_service.reserve_nearest_graspable(Position(0.5, 0.5, 0.5), 0.5) - assert cyl3.id == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.5) - assert "" == scene_service.reserve_nearest_graspable(Position(0.0, 0.0, 0.0), 0.5) - - scene_service.delete_all_collisions() - - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_side_obstacle.py b/src/python/arcor2_ur/tests/test_side_obstacle.py index 47e88f46..ae23c0a9 100644 --- a/src/python/arcor2_ur/tests/test_side_obstacle.py +++ b/src/python/arcor2_ur/tests/test_side_obstacle.py @@ -1,4 +1,3 @@ -import math import time import pytest @@ -19,12 +18,12 @@ def test_box(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 + X = 0.5 Y = 0.0 - Z = 0.2 - X = 0.9 + Z = 0.3 - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) box = Box("Box1", 0.2, 0.2, 0.95) @@ -55,12 +54,12 @@ def test_cylinder(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 + X = 0.5 Y = 0.0 - Z = 0.2 - X = 0.9 + Z = 0.3 - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) cyl = Cylinder("Cyl1", 0.1, 0.95) @@ -91,12 +90,12 @@ def test_sphere(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 + X = 0.5 Y = 0.0 - Z = 0.2 - X = 0.9 + Z = 0.3 - start_pose = Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)) + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) sphere = Sphere("Sphere1", 0.09) diff --git a/src/python/arcor2_ur/tests/test_suck_effector.py b/src/python/arcor2_ur/tests/test_suck_effector.py new file mode 100644 index 00000000..d46c1eb2 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_suck_effector.py @@ -0,0 +1,41 @@ +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.scripts.robot_publisher import load_robot_description +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(60) +def test_basics(start_processes: Urls) -> None: + # over, ze test bezi nad upravenym URDF modelom + robot_description = load_robot_description() + assert "suction_base_link" in robot_description + assert "suction_cup_link" in robot_description + assert "suction_tcp" in robot_description + assert "tool0_to_suction_base" in robot_description + assert "suction_base_to_cup" in robot_description + assert "suction_cup_to_tcp" in robot_description + + scene_service.URL = start_processes.robot_url + box = Box("UniqueBoxId", 0.1, 0.1, 0.1) + scene_service.upsert_collision(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1))) + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + + assert len(ot.robot_joints()) == 6 + + # ak je tool link nastaveny na suction_tcp, toto je prakticky runtime check, + # ze sa s tymto TCP vie realne pracovat + pos = ot.get_end_effector_pose("") + orig_z = pos.position.z + pos.position.z -= 0.05 + ot.move_to_pose("", pos, 0.5) + pos_after = ot.get_end_effector_pose("") + assert orig_z - pos_after.position.z > 0.045 + + ot.cleanup() From cee97de711089dd868cc76f9a35b6c58b5bc81c0 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Tue, 5 May 2026 23:19:57 +0200 Subject: [PATCH 07/11] feat: add mesh collision avoidance --- build-support/install_ur_dependencies.sh | 6 - src/python/arcor2_object_types/abstract.py | 4 +- src/python/arcor2_scene/scripts/scene.py | 50 +- src/python/arcor2_scene_data/scene_service.py | 8 +- src/python/arcor2_ur/debug/custom.rviz | 511 ++++++++++++++++++ src/python/arcor2_ur/object_types/ur5e.py | 34 +- src/python/arcor2_ur/scripts/ros_worker.py | 77 +-- src/python/arcor2_ur/scripts/ur.py | 52 +- src/python/arcor2_ur/tests/test_mesh.py | 53 ++ .../tests/test_mesh_object/taper.stl | 3 + .../arcor2_ur/tests/test_suck_effector.py | 6 +- 11 files changed, 709 insertions(+), 95 deletions(-) create mode 100644 src/python/arcor2_ur/debug/custom.rviz create mode 100644 src/python/arcor2_ur/tests/test_mesh.py create mode 100644 src/python/arcor2_ur/tests/test_mesh_object/taper.stl diff --git a/build-support/install_ur_dependencies.sh b/build-support/install_ur_dependencies.sh index d96e6e12..ff28ca6d 100755 --- a/build-support/install_ur_dependencies.sh +++ b/build-support/install_ur_dependencies.sh @@ -8,12 +8,6 @@ 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 \ -<<<<<<< HEAD ros-jazzy-ros-base=0.11.0-1noble.20260412.071950 \ ros-jazzy-ur=3.7.0-1noble.20260412.082316 \ ros-jazzy-moveit-py=2.12.4-1noble.20260412.073026 -======= - ros-jazzy-ros-base=0.11.0-1noble.20251108.003726 \ - ros-jazzy-ur=3.6.0-1noble.20251114.095610 \ - ros-jazzy-moveit-py=2.12.3-1noble.20251108.011222 ->>>>>>> 32612146 (feat(arcor2_ur): graspable obj representation, basic functionality tests) diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index 96c6b3a9..b5e1597b 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -198,7 +198,6 @@ class EffectorType(StrEnum): SUCK = "SUCK" -# TODO: zmenit na int class GraspableState(StrEnum): """Logical state of a graspable object in the scene. @@ -221,7 +220,7 @@ class GraspableState(StrEnum): WORLD = "WORLD" RESERVED = "RESERVED" - HIDEN = "HIDEN" + HIDDEN = "HIDDEN" ATTACHED = "ATTACHED" LOST = "LOST" @@ -241,6 +240,7 @@ class GraspableSource(StrEnum): CAMERA = "CAMERA" FIXED = "FIXED" + # TODO: remove OTHER = "OTHER" diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index 8104c6da..08ac5977 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -5,6 +5,7 @@ import math import random import time +from typing import NamedTuple import humps import numpy as np @@ -17,14 +18,19 @@ 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 -collision_objects: dict[str, CollisionSceneObject] = {} + +class CollisionObject(NamedTuple): + model: object_type.Models + pose: common.Pose + + +collision_objects: dict[str, CollisionObject] = {} started: bool = False inflation = 0.01 @@ -75,7 +81,7 @@ def put_box() -> RespT: content: application/json: schema: - type: object + $ref: Pose responses: 200: description: Ok @@ -91,11 +97,12 @@ def put_box() -> RespT: $ref: WebApiError """ - pose, metadata = parse_collision_body() + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") args = request.args.to_dict() box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"])) - collision_objects[box.id] = CollisionSceneObject(box, pose, metadata) + collision_objects[box.id] = CollisionObject(box, common.Pose.from_dict(humps.decamelize(request.json))) return jsonify("ok"), 200 @@ -124,7 +131,7 @@ def put_sphere() -> RespT: content: application/json: schema: - type: object + $ref: Pose responses: 200: description: Ok @@ -140,11 +147,12 @@ def put_sphere() -> RespT: $ref: WebApiError """ - pose, metadata = parse_collision_body() + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") args = humps.decamelize(request.args.to_dict()) sphere = object_type.Sphere(args["sphere_id"], float(args["radius"])) - collision_objects[sphere.id] = CollisionSceneObject(sphere, pose, metadata) + collision_objects[sphere.id] = CollisionObject(sphere, common.Pose.from_dict(humps.decamelize(request.json))) return jsonify("ok"), 200 @@ -177,7 +185,7 @@ def put_cylinder() -> RespT: content: application/json: schema: - type: object + $ref: Pose responses: 200: description: Ok @@ -193,11 +201,12 @@ def put_cylinder() -> RespT: $ref: WebApiError """ - pose, metadata = parse_collision_body() + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") args = humps.decamelize(request.args.to_dict()) cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"])) - collision_objects[cylinder.id] = CollisionSceneObject(cylinder, pose, metadata) + collision_objects[cylinder.id] = CollisionObject(cylinder, common.Pose.from_dict(humps.decamelize(request.json))) return jsonify("ok"), 200 @@ -242,7 +251,7 @@ def put_mesh() -> RespT: content: application/json: schema: - type: object + $ref: Pose responses: 200: description: Ok @@ -258,11 +267,12 @@ def put_mesh() -> RespT: $ref: WebApiError """ - pose, metadata = parse_collision_body() + if not isinstance(request.json, dict): + raise SceneGeneral("Body should be a JSON dict containing Pose.") args = humps.decamelize(request.args.to_dict()) mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) - collision_objects[mesh.id] = CollisionSceneObject(mesh, pose, metadata) + collision_objects[mesh.id] = CollisionObject(mesh, common.Pose.from_dict(humps.decamelize(request.json))) return jsonify("ok"), 200 @@ -409,25 +419,21 @@ def put_line_safe() -> RespT: arcor2/ros x forward, y left, z up unity x Right, y Up, z Forward """ - for obj_id, obj in collision_objects.items(): - model = obj.model - pose = obj.pose - + for obj_id, (model, pose) in collision_objects.items(): 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]) + 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: + else: # TODO mesh logger.warning(f"Unsupported type of collision model: {model.type()}.") continue diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py index f9679aa8..f5928285 100644 --- a/src/python/arcor2_scene_data/scene_service.py +++ b/src/python/arcor2_scene_data/scene_service.py @@ -6,7 +6,7 @@ from dataclasses_jsonschema import JsonSchemaMixin -from arcor2.data.common import Pose, Position +from arcor2.data.common import Pose from arcor2.data.object_type import Model3dType, Models from arcor2.data.scene import LineCheck, LineCheckResult, MeshFocusAction from arcor2.exceptions import Arcor2Exception @@ -31,12 +31,6 @@ class CollisionBody(JsonSchemaMixin): metadata: dict[str, Any] = field(default_factory=dict) -@dataclass -class ReserveGraspable(JsonSchemaMixin): - position: Position - radius: float - - @dataclass class MeshParameters(JsonSchemaMixin): mesh_scale_x: float = 1.0 diff --git a/src/python/arcor2_ur/debug/custom.rviz b/src/python/arcor2_ur/debug/custom.rviz new file mode 100644 index 00000000..e68cafe4 --- /dev/null +++ b/src/python/arcor2_ur/debug/custom.rviz @@ -0,0 +1,511 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.5 + Tree Height: 373 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 160 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_cup_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + base_link_inertia: + Value: true + flange: + Value: true + forearm_link: + Value: true + ft_frame: + Value: true + shoulder_link: + Value: true + suction_base_link: + Value: true + suction_cup_link: + Value: true + suction_tcp: + Value: true + tool0: + Value: true + upper_arm_link: + Value: true + world: + Value: true + wrist_1_link: + Value: true + wrist_2_link: + Value: true + wrist_3_link: + Value: true + Marker Scale: 0.20000000298023224 + Name: TF + Show Arrows: false + Show Axes: false + Show Names: false + Tree: + world: + base_link: + base: + {} + base_link_inertia: + shoulder_link: + upper_arm_link: + forearm_link: + wrist_1_link: + wrist_2_link: + wrist_3_link: + flange: + tool0: + suction_base_link: + suction_cup_link: + suction_tcp: + {} + ft_frame: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_cup_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: ur_manipulator + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_cup_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + suction_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.4266345500946045 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0.4000000059604645 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6399993896484375 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 2.0200021266937256 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1347 + Hide Left Dock: true + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001c9000003aafc0200000005fb000000100044006900730070006c006100790073000000003b000003aa000000c700fffffffb0000000a00560069006500770073000000015a000000a0000000a000fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000015f0000020a0000016900ffffff000009f4000004ed00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 2548 + X: 0 + Y: 0 diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index 2d7cfb9b..e6e9b86e 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -138,12 +138,27 @@ def pick_up_object( position: Position, radius: float, effector_type: EffectorType = EffectorType.SUCK, - grasp_positions: None | list[GraspPosition] = None, - object_type_name: None | str = None, + grasp_positions: list[GraspPosition] | None = None, + object_type_name: str = "", speed: float = 50.0, safe: bool = True, payload: float = 0.0, + *, + an: None | str = None, ) -> None: + """Picks up a graspable object from the selected area. + + :param position: Center of the area where the object should be found. + :param radius: Search radius. + :param effector_type: Type of end effector used for grasping. + :param grasp_position: Preferred grasp position. + :param object_type_name: Optional object model type filter. + :param speed: Relative speed. + :param safe: Avoid collisions. + :param payload: Object weight. + :return: + """ + assert radius >= 0.0 assert 0.0 <= speed <= 100.0 assert 0.0 <= payload <= 5.0 @@ -160,7 +175,7 @@ def pick_up_object( radius=radius, effector_type=effector_type, grasp_positions=grasp_positions, - object_type_name=object_type_name, + object_type_name=object_type_name or None, velocity=speed, payload=payload, safe=safe, @@ -173,7 +188,18 @@ def place_object( speed: float = 50.0, safe: bool = True, payload: float = 0.0, + *, + an: None | str = None, ) -> None: + """Places the currently attached object to the target pose. + + :param pose: Target pose of the placed object. + :param speed: Relative speed. + :param safe: Avoid collisions. + :param payload: Object weight. + :return: + """ + assert 0.0 <= speed <= 100.0 assert 0.0 <= payload <= 5.0 @@ -266,3 +292,5 @@ def inverse_kinematics( ) move.__action__ = ActionMetadata() # type: ignore + pick_up_object.__action__ = ActionMetadata() # type: ignore + place_object.__action__ = ActionMetadata() # type: ignore diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index dc7f5acc..e8385948 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -29,6 +29,7 @@ from arcor2.data.robot import InverseKinematicsRequest from arcor2.logging import get_logger from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_storage import client as storage_client from arcor2_ur import topics from arcor2_ur.common import CollisionSceneObject from arcor2_ur.exceptions import UrGeneral @@ -92,33 +93,50 @@ def wait_for_future(future, *, timeout_sec: float = 2.0): return future.result() +# remove id ? def create_collision_object( - obj: CollisionSceneObject, obj_id: str, frame_id: str, pose_in_frame: Pose, attached_to_link: str | None = None -) -> CollisionObject | AttachedCollisionObject | None: - prim = SolidPrimitive() + obj: CollisionSceneObject, + obj_id: str, + frame_id: str, + pose_in_frame: Pose, + attached_to_link: str | None = None, +) -> CollisionObject | AttachedCollisionObject: + + collision_object = CollisionObject() + collision_object.header.frame_id = frame_id + collision_object.id = obj_id + collision_object.operation = CollisionObject.ADD if isinstance(obj.model, object_type.Box): + prim = SolidPrimitive() prim.type = SolidPrimitive.BOX prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) + elif isinstance(obj.model, object_type.Sphere): + prim = SolidPrimitive() prim.type = SolidPrimitive.SPHERE prim.dimensions = [obj.model.radius] + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) + elif isinstance(obj.model, object_type.Cylinder): + prim = SolidPrimitive() prim.type = SolidPrimitive.CYLINDER prim.dimensions = [obj.model.height, obj.model.radius] - else: - logger.warning(f"Unsupported collision model for MoveIt scene: {obj.model.type()}. Skipping {obj_id}.") - return None + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) - collision_object = CollisionObject() - collision_object.header.frame_id = frame_id - collision_object.id = obj_id - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) - collision_object.operation = CollisionObject.ADD + # mal by fungovat subor + elif isinstance(obj.model, object_type.Mesh): + mesh = storage_client.get_asset_data(obj.model.asset_id) + + collision_object.meshes.append(mesh) + collision_object.mesh_poses.append(pose_to_ros_pose(pose_in_frame)) if attached_to_link is not None: attached = AttachedCollisionObject() @@ -278,10 +296,6 @@ def compute_offset_pose_in_tool_axis(pose: Pose, distance: float) -> Pose: def compute_target_eef_pose(target_object_pose: Pose, attached_pose: Pose) -> Pose: - """Computes the target EEF pose so that the attached object ends up at target_object_pose. - target_object_pose: World pose where the object should be. - attached_pose: Pose of the object relative to the EEF (T_eef_obj). - """ return tr.make_pose_abs(target_object_pose, attached_pose.inversed()) @@ -789,14 +803,14 @@ def apply_collision_objects(self, scene) -> None: for obj_id, obj in self.collision_objects.items(): state = obj.metadata.get("state") - if state == GraspableState.LOST.value: + if state == GraspableState.LOST: continue - if state == GraspableState.HIDEN.value: + if state == GraspableState.HIDDEN: continue # TODO: choose it by id - if state == GraspableState.ATTACHED.value: + if state == GraspableState.ATTACHED: attached_pose_dict = obj.metadata.get("attached_pose") if attached_pose_dict is None: logger.warning(f"Attached object {obj_id} is missing attached_pose. Skipping.") @@ -905,7 +919,7 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]: def get_attached_object_id(self) -> str | None: for obj_id, obj in self.collision_objects.items(): - if obj.metadata.get("state") == GraspableState.ATTACHED.value: + if obj.metadata.get("state") == GraspableState.ATTACHED: return obj_id return None @@ -917,7 +931,7 @@ def attach_object( velocity: float, payload: float, safe: bool, - ) -> None: + ) -> dict: object = self.collision_objects[object_id] object.metadata.pop("attached_pose", None) @@ -926,7 +940,7 @@ def attach_object( try: self.move_to_pose(pre_grasp_pose, velocity, payload, safe) - object.metadata["state"] = GraspableState.HIDEN.value + object.metadata["state"] = GraspableState.HIDDEN if self.tool: # Skip vacuum activation/check when no physical tool is configured self.suck(60) @@ -940,19 +954,18 @@ def attach_object( self.release() raise UrGeneral(f"Failed to attach object {object_id}: vacuum not reached.") - # TODO: send info to ur - object.metadata["state"] = GraspableState.ATTACHED.value + object.metadata["state"] = GraspableState.ATTACHED attached_pose = tr.make_pose_rel(grasp_pose, object.pose) object.metadata["attached_pose"] = attached_pose.to_dict() except Exception: continue - return + return object.metadata.copy() raise UrGeneral(f"Unable to attach object {object_id}.") - def detach_object(self, target_pose: Pose, velocity: float, payload: float, safe: bool) -> None: + def detach_object(self, target_pose: Pose, velocity: float, payload: float, safe: bool) -> dict[str, Any]: object_id = self.get_attached_object_id() if object_id is None: raise UrGeneral("No object to detach.") @@ -973,16 +986,18 @@ def detach_object(self, target_pose: Pose, velocity: float, payload: float, safe self.remove_object(object_id) - object.metadata["state"] = GraspableState.HIDEN.value + object.metadata["state"] = GraspableState.HIDDEN object.metadata.pop("attached_pose", None) self.update_collisions(self.collision_objects) object.pose = target_pose self.move_to_pose(post_detach_pose, velocity, payload, safe) - object.metadata["state"] = GraspableState.WORLD.value + object.metadata["state"] = GraspableState.WORLD self.update_collisions(self.collision_objects) + return {"object_id": object_id, "state": object.metadata["state"]} + def remove_object(self, object_id: str) -> None: moveitpy, _ = self._get_moveit() co = CollisionObject() @@ -1058,7 +1073,7 @@ def ros_worker_main( pose = Pose.from_dict(kwargs["pose"]) result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"]) elif op == "pick_up_object": - runtime.attach_object( + result = runtime.attach_object( kwargs["object_id"], EffectorType(kwargs["effector_type"]), [GraspPosition(gp) for gp in kwargs["grasp_positions"]], @@ -1066,16 +1081,14 @@ def ros_worker_main( kwargs["payload"], kwargs["safe"], ) - result = {} elif op == "place_object": pose = Pose.from_dict(kwargs["pose"]) - runtime.detach_object( + result = runtime.detach_object( pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"], ) - result = {} elif op == "get_joints": result = runtime.get_joints() elif op == "ik": diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index cfb9f758..a10b2653 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -3,9 +3,12 @@ import argparse import logging import os +import shutil +import tempfile from dataclasses import dataclass, field from functools import wraps from importlib import resources +from pathlib import Path import humps from ament_index_python.packages import get_package_share_directory # pants: no-infer-dep @@ -65,11 +68,16 @@ def load_custom_urdf() -> str: text = urdf_res.read_text(encoding="utf-8") - with resources.as_file(meshes_res) as meshes_dir: - text = text.replace( - "package://meshes/", - f"file://{meshes_dir.as_posix()}/", - ) + meshes_cache_dir = Path(tempfile.gettempdir()) / f"arcor2_ur_meshes_{version()}" + + if not meshes_cache_dir.exists(): + with resources.as_file(meshes_res) as meshes_dir: + shutil.copytree(meshes_dir, meshes_cache_dir, dirs_exist_ok=True) + + text = text.replace( + "package://meshes/", + f"file://{meshes_cache_dir.as_posix()}/", + ) return text @@ -244,7 +252,7 @@ def pick_up_object() -> RespT: type: array items: type: string - model3d_type: + object_type_name: type: string default: none velocity: @@ -282,16 +290,16 @@ def pick_up_object() -> RespT: if radius < 0.0: raise UrGeneral("Radius has to be >= 0.") - effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK.value)) + effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK)) grasp_positions = body.get( "grasp_positions", [ - GraspPosition.TOP.value, - GraspPosition.RIGHT.value, - GraspPosition.LEFT.value, - GraspPosition.FRONT.value, - GraspPosition.BACK.value, - GraspPosition.BOTTOM.value, + GraspPosition.TOP, + GraspPosition.RIGHT, + GraspPosition.LEFT, + GraspPosition.FRONT, + GraspPosition.BACK, + GraspPosition.BOTTOM, GraspPosition.ALL, ], ) @@ -309,7 +317,7 @@ def pick_up_object() -> RespT: if obj.metadata.get("object_type") != "graspable": continue - if obj.metadata.get("state") != GraspableState.WORLD.value: + if obj.metadata.get("state") != GraspableState.WORLD: continue if object_type_cls is not None and not isinstance(obj.model, object_type_cls): @@ -328,13 +336,13 @@ def pick_up_object() -> RespT: selected = globs.collision_objects[nearest_id] previous_state = selected.metadata.get("state") - selected.metadata["state"] = GraspableState.RESERVED.value + selected.metadata["state"] = GraspableState.RESERVED assert globs.state try: globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) - globs.state.worker.request( + metadata = globs.state.worker.request( "pick_up_object", object_id=nearest_id, effector_type=effector_type, @@ -343,6 +351,9 @@ def pick_up_object() -> RespT: payload=payload, safe=safe, ) + + selected.metadata.clear() + selected.metadata.update(metadata) except Exception: selected.metadata["state"] = previous_state globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) @@ -395,7 +406,7 @@ def place_object() -> RespT: safe = bool(body.get("safe", True)) assert globs.state - globs.state.worker.request( + update = globs.state.worker.request( "place_object", pose=pose.to_dict(), velocity=velocity, @@ -403,6 +414,11 @@ def place_object() -> RespT: safe=safe, ) + obj = globs.collision_objects[update["object_id"]] + obj.metadata["state"] = update["state"] + obj.metadata.pop("attached_pose", None) + obj.pose = pose + return Response(status=204) @@ -631,8 +647,6 @@ def put_mesh() -> RespT: globs.collision_objects[mesh.id] = CollisionSceneObject(mesh, pose, metadata) - logger.warning("Mesh ignored (only boxes supported).") - if started(): assert globs.state globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) diff --git a/src/python/arcor2_ur/tests/test_mesh.py b/src/python/arcor2_ur/tests/test_mesh.py new file mode 100644 index 00000000..5dde7281 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_mesh.py @@ -0,0 +1,53 @@ +import time +from pathlib import Path + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Mesh +from arcor2_scene_data import scene_service +from arcor2_storage import client as storage_client +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + +MESH_PATH = Path(__file__).parent / "test_mesh_object" / "taper.stl" +MESH_ASSET_ID = "taper.stl" + + +@pytest.mark.timeout(321) +def test_mesh_collision_avoidance(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + + storage_client.create_asset( + MESH_ASSET_ID, MESH_PATH.read_bytes(), description="Test mesh for UR5e collision avoidance." + ) + + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + + ot.move_to_pose("", start_pose, 0.3, safe=False) + + mesh = Mesh("Mesh1", MESH_ASSET_ID) + mesh_pose = Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)) + + scene_service.upsert_collision(mesh, mesh_pose) + assert mesh.id in scene_service.collision_ids() + time.sleep(100) + + ot.move_to_pose("", goal_pose, 0.3, safe=True) + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_mesh_object/taper.stl b/src/python/arcor2_ur/tests/test_mesh_object/taper.stl new file mode 100644 index 00000000..3c94a3d0 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_mesh_object/taper.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2faa852002f4d06edc18faf5f9de3753e5e46d9349f75acea44812ac25b28a2c +size 249334 diff --git a/src/python/arcor2_ur/tests/test_suck_effector.py b/src/python/arcor2_ur/tests/test_suck_effector.py index d46c1eb2..e45ea741 100644 --- a/src/python/arcor2_ur/tests/test_suck_effector.py +++ b/src/python/arcor2_ur/tests/test_suck_effector.py @@ -9,8 +9,8 @@ @pytest.mark.timeout(60) -def test_basics(start_processes: Urls) -> None: - # over, ze test bezi nad upravenym URDF modelom +def test_suck_effector(start_processes: Urls) -> None: + robot_description = load_robot_description() assert "suction_base_link" in robot_description assert "suction_cup_link" in robot_description @@ -29,8 +29,6 @@ def test_basics(start_processes: Urls) -> None: assert len(ot.robot_joints()) == 6 - # ak je tool link nastaveny na suction_tcp, toto je prakticky runtime check, - # ze sa s tymto TCP vie realne pracovat pos = ot.get_end_effector_pose("") orig_z = pos.position.z pos.position.z -= 0.05 From 3d08d3425651fce474f6d1f3bb9ceeb821c521a5 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Sat, 9 May 2026 22:04:03 +0200 Subject: [PATCH 08/11] feat: optimize ros worker grasping Add new grasp planner, update ROS worker grasp flow, and add pick-up tests. --- .github/workflows/pants.yaml | 7 +- src/python/arcor2_object_types/abstract.py | 6 +- src/python/arcor2_scene/scripts/scene.py | 21 +- src/python/arcor2_scene_data/scene_service.py | 14 +- src/python/arcor2_ur/data/BUILD | 1 + .../ur5e/collision/stary_suction_cup_1cup.stl | 3 + .../meshes/ur5e/visual/base_link_1cup.stl | 3 + .../meshes/ur5e/visual/suction_cup_1cup.stl | 3 + src/python/arcor2_ur/data/urdf/ur5e.srdf | 20 + src/python/arcor2_ur/data/urdf/ur5e.urdf | 2 +- src/python/arcor2_ur/debug/custom.rviz | 10 +- src/python/arcor2_ur/object_types/ur5e.py | 95 +- src/python/arcor2_ur/scripts/BUILD | 6 + src/python/arcor2_ur/scripts/grasp_planner.py | 233 + src/python/arcor2_ur/scripts/ros_worker.py | 551 +- src/python/arcor2_ur/scripts/ur.py | 143 +- src/python/arcor2_ur/tests/BUILD | 28 +- src/python/arcor2_ur/tests/conftest.py | 76 +- .../arcor2_ur/tests/results_velka spatna.txt | 28886 ++++++++++++++++ .../test_collision/test_obstacle_graspable.py | 57 + .../test_overhead_obstacle_box.py | 55 + .../test_overhead_obstacle_cylinder.py | 55 + .../test_overhead_obstacle_mesh.py | 70 + .../test_overhead_obstacle_sphere.py | 55 + .../test_collision/test_side_obstacle_box.py | 45 + .../test_side_obstacle_cylinder.py | 45 + .../test_side_obstacle_mesh.py} | 41 +- .../test_side_obstacle_sphere.py | 65 + .../tests/test_graspable_collision_object.py | 59 - .../tests/test_mesh_object/taper.stl | 3 - .../tests/test_mesh_object/triangle_block.stl | 3 + .../arcor2_ur/tests/test_overhead_obstacle.py | 147 - .../test_pick_and_place_box_by_id.py | 40 + .../test_pick_and_place_box_by_position.py | 40 + .../test_pick_and_place_cylinder_by_id.py | 40 + ...est_pick_and_place_cylinder_by_position.py | 40 + .../test_pick_and_place_mesh_by_id.py | 55 + .../test_pick_and_place_mesh_by_position.py | 54 + .../test_pick_and_place_rotated_box.py | 40 + .../test_pick_and_place_sphere_by_id.py | 40 + .../test_pick_and_place_sphere_by_position.py | 40 + .../test_pick_up/test_pick_up_box_by_id.py | 35 + .../test_pick_up_box_by_position.py} | 15 +- .../test_pick_up_cylinder_by_id.py | 35 + .../test_pick_up_cylinder_by_position.py | 35 + .../test_pick_up/test_pick_up_mesh_by_id.py | 50 + .../test_pick_up_mesh_by_position.py | 50 + .../test_pick_up/test_pick_up_sphere_by_id.py | 35 + .../test_pick_up_sphere_by_position.py | 35 + .../arcor2_ur/tests/test_side_obstacle.py | 137 - .../arcor2_ur/tests/test_suck_effector.py | 5 +- 51 files changed, 30971 insertions(+), 658 deletions(-) create mode 100644 src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl create mode 100644 src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl create mode 100644 src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl create mode 100644 src/python/arcor2_ur/data/urdf/ur5e.srdf create mode 100644 src/python/arcor2_ur/scripts/grasp_planner.py create mode 100644 src/python/arcor2_ur/tests/results_velka spatna.txt create mode 100644 src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py create mode 100644 src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py create mode 100644 src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py create mode 100644 src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py create mode 100644 src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py create mode 100644 src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py create mode 100644 src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py rename src/python/arcor2_ur/tests/{test_mesh.py => test_collision/test_side_obstacle_mesh.py} (53%) create mode 100644 src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py delete mode 100644 src/python/arcor2_ur/tests/test_graspable_collision_object.py delete mode 100644 src/python/arcor2_ur/tests/test_mesh_object/taper.stl create mode 100644 src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl delete mode 100644 src/python/arcor2_ur/tests/test_overhead_obstacle.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py rename src/python/arcor2_ur/tests/{test_move_object.py => test_pick_up/test_pick_up_box_by_position.py} (61%) create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py delete mode 100644 src/python/arcor2_ur/tests/test_side_obstacle.py diff --git a/.github/workflows/pants.yaml b/.github/workflows/pants.yaml index 847e2d4e..e9bbc43a 100644 --- a/.github/workflows/pants.yaml +++ b/.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 with retries + run: | + source /opt/ros/jazzy/setup.bash + pants --changed-since=origin/master --changed-dependents=transitive --tag='unstable_ros' test --test-attempts-default=5 - name: Build Python packages run: | pants --filter-target-type=python_distribution package :: diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index b5e1597b..7a7dcc0b 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -214,15 +214,15 @@ class GraspableState(StrEnum): ATTACHED Attached to the robot end-effector. - LOST - Object not observable. Location unknown. + TOUCH_ALLOWED + Object acts as a collision obstacle, except for selected end-effector links. """ WORLD = "WORLD" RESERVED = "RESERVED" HIDDEN = "HIDDEN" ATTACHED = "ATTACHED" - LOST = "LOST" + TOUCH_ALLOWED = "TOUCH_ALLOWED" class GraspableSource(StrEnum): diff --git a/src/python/arcor2_scene/scripts/scene.py b/src/python/arcor2_scene/scripts/scene.py index 08ac5977..a9b4fef4 100644 --- a/src/python/arcor2_scene/scripts/scene.py +++ b/src/python/arcor2_scene/scripts/scene.py @@ -18,6 +18,7 @@ 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 parse_collision_body from arcor2_web.flask import Response, RespT, create_app, run_app app = create_app(__name__) @@ -97,12 +98,11 @@ def put_box() -> RespT: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") + pose, _ = 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] = CollisionObject(box, pose) return jsonify("ok"), 200 @@ -147,12 +147,11 @@ def put_sphere() -> RespT: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") + pose, _ = 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] = CollisionObject(sphere, pose) return jsonify("ok"), 200 @@ -201,12 +200,11 @@ def put_cylinder() -> RespT: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") + pose, _ = 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] = CollisionObject(cylinder, pose) return jsonify("ok"), 200 @@ -267,12 +265,11 @@ def put_mesh() -> RespT: $ref: WebApiError """ - if not isinstance(request.json, dict): - raise SceneGeneral("Body should be a JSON dict containing Pose.") + pose, _ = 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] = CollisionObject(mesh, pose) return jsonify("ok"), 200 diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py index f5928285..a28466ea 100644 --- a/src/python/arcor2_scene_data/scene_service.py +++ b/src/python/arcor2_scene_data/scene_service.py @@ -6,7 +6,7 @@ from dataclasses_jsonschema import JsonSchemaMixin -from arcor2.data.common import Pose +from arcor2.data.common import Pose, Position from arcor2.data.object_type import Model3dType, Models from arcor2.data.scene import LineCheck, LineCheckResult, MeshFocusAction from arcor2.exceptions import Arcor2Exception @@ -152,6 +152,16 @@ def upsert_graspable( rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params) +@handle(SceneServiceException, logger, message="Failed to get graspable object state.") +def graspable_state(object_id: str) -> GraspableState: + return GraspableState(rest.call(rest.Method.GET, f"{URL}/graspable/{object_id}/state", return_type=str)) + + +@handle(SceneServiceException, logger, message="Failed to get graspable object position.") +def graspable_position(object_id: str) -> Position: + return rest.call(rest.Method.GET, f"{URL}/graspable/{object_id}/position", return_type=Position) + + @handle(SceneServiceException, logger, message="Failed to delete the collision.") def delete_collision_id(collision_id: str) -> None: rest.call(rest.Method.DELETE, f"{URL}/collisions/{collision_id}") @@ -235,6 +245,8 @@ def world_pose(transform_id: str) -> Pose: SceneServiceException.__name__, upsert_collision.__name__, upsert_graspable.__name__, + graspable_state.__name__, + graspable_position.__name__, delete_collision_id.__name__, collision_ids.__name__, focus.__name__, diff --git a/src/python/arcor2_ur/data/BUILD b/src/python/arcor2_ur/data/BUILD index 7c8b8393..0d71e86a 100644 --- a/src/python/arcor2_ur/data/BUILD +++ b/src/python/arcor2_ur/data/BUILD @@ -7,6 +7,7 @@ resources( name="urdf_assets", sources=[ "urdf/ur5e.urdf", + "urdf/ur5e.srdf", "urdf/meshes/ur5e/collision/*.stl", "urdf/meshes/ur5e/visual/*.dae", ], diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl new file mode 100644 index 00000000..67b71ffa --- /dev/null +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/stary_suction_cup_1cup.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:10686a45842b55d7ce5dc6d3dbf1e51b235a1722e4697e4cec2ccf22e65bbfc4 +size 250034 diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl new file mode 100644 index 00000000..17bc267e --- /dev/null +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/base_link_1cup.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:521d044495d657ab0f843fec69d3b9025477c3f139fb6b0060b0c9fca682865a +size 750084 diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl new file mode 100644 index 00000000..547804c8 --- /dev/null +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/visual/suction_cup_1cup.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:160de600a401f908bec47d7f8553adbc926e4e09a0d3cc16b508d16e2353a3e4 +size 250084 diff --git a/src/python/arcor2_ur/data/urdf/ur5e.srdf b/src/python/arcor2_ur/data/urdf/ur5e.srdf new file mode 100644 index 00000000..3e6805bd --- /dev/null +++ b/src/python/arcor2_ur/data/urdf/ur5e.srdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/python/arcor2_ur/data/urdf/ur5e.urdf b/src/python/arcor2_ur/data/urdf/ur5e.urdf index ad38555b..5699a9f0 100644 --- a/src/python/arcor2_ur/data/urdf/ur5e.urdf +++ b/src/python/arcor2_ur/data/urdf/ur5e.urdf @@ -314,7 +314,7 @@ - + diff --git a/src/python/arcor2_ur/debug/custom.rviz b/src/python/arcor2_ur/debug/custom.rviz index e68cafe4..60938c92 100644 --- a/src/python/arcor2_ur/debug/custom.rviz +++ b/src/python/arcor2_ur/debug/custom.rviz @@ -496,16 +496,16 @@ Visualization Manager: Window Geometry: Displays: collapsed: true - Height: 1347 + Height: 652 Hide Left Dock: true Hide Right Dock: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001c9000003aafc0200000005fb000000100044006900730070006c006100790073000000003b000003aa000000c700fffffffb0000000a00560069006500770073000000015a000000a0000000a000fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000015f0000020a0000016900ffffff000009f4000004ed00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001c9000003aafc0200000005fb000000100044006900730070006c006100790073000000003b000003aa000000c700fffffffb0000000a00560069006500770073000000015a000000a0000000a000fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000015f0000020a0000016900ffffff000003170000023600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 2548 - X: 0 - Y: 0 + Width: 791 + X: 98 + Y: 24 diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index e6e9b86e..4b682b13 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -4,6 +4,7 @@ from dataclasses_jsonschema import JsonSchemaMixin +from arcor2.data import object_type from arcor2.data.common import ActionMetadata, Joint, Pose, Position, StrEnum from arcor2.data.robot import InverseKinematicsRequest from arcor2_object_types.abstract import EffectorType, GraspPosition, Robot, Settings @@ -25,12 +26,22 @@ def avg(self) -> float: @dataclass -class PickUpObject(JsonSchemaMixin): +class PickUpObjectByPosition(JsonSchemaMixin): position: Position radius: float effector_type: EffectorType = EffectorType.SUCK grasp_positions: list[GraspPosition] = field(default_factory=lambda: [GraspPosition.ALL]) - object_type_name: None | str = None + object_type_name: object_type.Model3dType | None = None + velocity: float = 50.0 + payload: float = 0.0 + safe: bool = True + + +@dataclass +class PickUpObjectById(JsonSchemaMixin): + object_id: str + effector_type: EffectorType = EffectorType.SUCK + grasp_positions: list[GraspPosition] = field(default_factory=lambda: [GraspPosition.ALL]) velocity: float = 50.0 payload: float = 0.0 safe: bool = True @@ -99,9 +110,18 @@ def get_end_effector_pose(self, end_effector_id: str) -> Pose: return rest.call(rest.Method.GET, f"{self.settings.url}/eef/pose", return_type=Pose) def move_to_pose( - self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, linear: bool = True + self, + end_effector_id: str, + target_pose: Pose, + speed: float, + safe: bool = True, + linear: bool = True, ) -> None: - self.move(target_pose, speed * 100, safe) + self.move( + target_pose, + speed * 100, + safe=safe, + ) def move( self, @@ -130,16 +150,21 @@ def move( rest.Method.PUT, f"{self.settings.url}/eef/pose", body=pose, - params={"velocity": speed, "payload": payload, "safe": safe}, + timeout=rest.Timeout(read=120), + params={ + "velocity": speed, + "payload": payload, + "safe": safe, + }, ) - def pick_up_object( + def pick_up_object_by_position( self, position: Position, radius: float, effector_type: EffectorType = EffectorType.SUCK, grasp_positions: list[GraspPosition] | None = None, - object_type_name: str = "", + object_type_name: object_type.Model3dType | None = None, speed: float = 50.0, safe: bool = True, payload: float = 0.0, @@ -151,7 +176,7 @@ def pick_up_object( :param position: Center of the area where the object should be found. :param radius: Search radius. :param effector_type: Type of end effector used for grasping. - :param grasp_position: Preferred grasp position. + :param grasp_positions: Preferred grasp positions. :param object_type_name: Optional object model type filter. :param speed: Relative speed. :param safe: Avoid collisions. @@ -169,17 +194,61 @@ def pick_up_object( with self._move_lock: rest.call( rest.Method.PUT, - f"{self.settings.url}/graspable/pick_up_object", - body=PickUpObject( + f"{self.settings.url}/graspable/pick_up_object_by_position", + body=PickUpObjectByPosition( position=position, radius=radius, effector_type=effector_type, grasp_positions=grasp_positions, - object_type_name=object_type_name or None, + object_type_name=object_type_name, + velocity=speed, + payload=payload, + safe=safe, + ), + timeout=rest.Timeout(read=120), + ) + + def pick_up_object_by_id( + self, + object_id: str, + effector_type: EffectorType = EffectorType.SUCK, + grasp_positions: list[GraspPosition] | None = None, + speed: float = 50.0, + safe: bool = True, + payload: float = 0.0, + *, + an: None | str = None, + ) -> None: + """Picks up a graspable object by its ID. + + :param object_id: Collision/graspable object ID. + :param effector_type: Type of end effector used for grasping. + :param grasp_positions: Preferred grasp positions. + :param speed: Relative speed. + :param safe: Avoid collisions. + :param payload: Object weight. + :return: + """ + + assert 0.0 <= speed <= 100.0 + assert 0.0 <= payload <= 5.0 + + if grasp_positions is None: + grasp_positions = [GraspPosition.ALL] + + with self._move_lock: + rest.call( + rest.Method.PUT, + f"{self.settings.url}/graspable/pick_up_object_by_id", + body=PickUpObjectById( + object_id=object_id, + effector_type=effector_type, + grasp_positions=grasp_positions, velocity=speed, payload=payload, safe=safe, ), + timeout=rest.Timeout(read=120), ) def place_object( @@ -213,6 +282,7 @@ def place_object( payload=payload, safe=safe, ), + timeout=rest.Timeout(read=120), ) def suck( @@ -292,5 +362,6 @@ def inverse_kinematics( ) move.__action__ = ActionMetadata() # type: ignore - pick_up_object.__action__ = ActionMetadata() # type: ignore + pick_up_object_by_position.__action__ = ActionMetadata() # type: ignore + pick_up_object_by_id.__action__ = ActionMetadata() # type: ignore place_object.__action__ = ActionMetadata() # type: ignore diff --git a/src/python/arcor2_ur/scripts/BUILD b/src/python/arcor2_ur/scripts/BUILD index a909b292..28b15d68 100644 --- a/src/python/arcor2_ur/scripts/BUILD +++ b/src/python/arcor2_ur/scripts/BUILD @@ -1,5 +1,11 @@ python_sources( + sources=[ + "*.py", + ], dependencies=[ + "3rdparty#PyYAML", + "3rdparty#numpy", + "3rdparty#open3d", "src/python/arcor2_ur/data:moveit_yaml", "src/python/arcor2_ur/data:urdf_assets", ], diff --git a/src/python/arcor2_ur/scripts/grasp_planner.py b/src/python/arcor2_ur/scripts/grasp_planner.py new file mode 100644 index 00000000..99a2a963 --- /dev/null +++ b/src/python/arcor2_ur/scripts/grasp_planner.py @@ -0,0 +1,233 @@ +import math + +import numpy as np +import open3d as o3d +from shape_msgs.msg import Mesh as RosMesh # pants: no-infer-dep + +from arcor2.data import object_type +from arcor2.data.common import Orientation, Pose, Position +from arcor2_object_types.abstract import EffectorType, GraspPosition +from arcor2_ur.common import CollisionSceneObject + +PRE_GRASP_OFFSET = 0.2 +GRASP_OFFSET = -0.01 + + +def generate_grasp_poses( + obj: CollisionSceneObject, + effector_type: EffectorType, + grasp_positions: list[GraspPosition] | None, + ros_mesh: RosMesh | None = None, +) -> list[tuple[Pose, Pose]]: + + mesh = object_to_mesh(obj, ros_mesh) + + geometry = filter_geometry_by_grasp_position(mesh, grasp_positions) + + candidates = create_grasp_candidates_from_surfaces(geometry, effector_type) + + return candidates[:20] + + +def object_to_mesh(obj: CollisionSceneObject, ros_mesh: RosMesh | None) -> o3d.geometry.TriangleMesh: + """Box/Cylinder/Sphere/Mesh -> Open3D world mesh.""" + + model = obj.model + + if isinstance(model, object_type.Box): + mesh = o3d.geometry.TriangleMesh.create_box(width=model.size_x, height=model.size_y, depth=model.size_z) + mesh.translate((-model.size_x / 2, -model.size_y / 2, -model.size_z / 2)) + + elif isinstance(model, object_type.Cylinder): + mesh = o3d.geometry.TriangleMesh.create_cylinder(radius=model.radius, height=model.height) + + elif isinstance(model, object_type.Sphere): + mesh = o3d.geometry.TriangleMesh.create_sphere(radius=model.radius) + + elif isinstance(model, object_type.Mesh): + if ros_mesh is None: + raise ValueError("Object is Mesh, but ros_mesh is None.") + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector([(vertex.x, vertex.y, vertex.z) for vertex in ros_mesh.vertices]) + mesh.triangles = o3d.utility.Vector3iVector([tuple(triangle.vertex_indices) for triangle in ros_mesh.triangles]) + + else: + raise ValueError(f"Unsupported object model: {type(model).__name__}") + + apply_pose_to_mesh(mesh, obj.pose) + mesh.compute_triangle_normals() + mesh.compute_vertex_normals() + return mesh + + +def apply_pose_to_mesh(mesh: o3d.geometry.TriangleMesh, pose: Pose) -> None: + q = pose.orientation + rotation = quaternion_to_rotation_matrix(q) + + mesh.rotate(rotation, center=(0.0, 0.0, 0.0)) + mesh.translate((pose.position.x, pose.position.y, pose.position.z)) + + +def quaternion_to_rotation_matrix(q: Orientation) -> np.ndarray: + x, y, z, w = q.x, q.y, q.z, q.w + + norm = math.sqrt(x * x + y * y + z * z + w * w) + if norm == 0.0: + raise ValueError("Invalid quaternion.") + + x, y, z, w = x / norm, y / norm, z / norm, w / norm + + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ] + ) + + +def filter_geometry_by_grasp_position( + mesh: o3d.geometry.TriangleMesh, + grasp_positions: list[GraspPosition] | None, +) -> o3d.geometry.TriangleMesh: + """TOP/BOTTOM/LEFT/RIGHT/FRONT/BACK filter.""" + + if grasp_positions is None or GraspPosition.ALL in grasp_positions: + return mesh + + mesh.compute_triangle_normals() + + triangles = np.asarray(mesh.triangles) + normals = np.asarray(mesh.triangle_normals) + + wanted_dirs = { + GraspPosition.TOP: (0.0, 0.0, 1.0), + GraspPosition.BOTTOM: (0.0, 0.0, -1.0), + GraspPosition.RIGHT: (1.0, 0.0, 0.0), + GraspPosition.LEFT: (-1.0, 0.0, 0.0), + GraspPosition.BACK: (0.0, 1.0, 0.0), + GraspPosition.FRONT: (0.0, -1.0, 0.0), + } + + kept = [] + for triangle, normal in zip(triangles, normals): + for pos in grasp_positions: + direction = wanted_dirs.get(pos) + if ( + direction is not None and dot(normal, direction) >= 0.707 + ): # maximum allowed rotation from the original normal: 45° + kept.append(triangle) + break + + filtered = o3d.geometry.TriangleMesh() + filtered.vertices = mesh.vertices + filtered.triangles = o3d.utility.Vector3iVector(kept) + filtered.compute_triangle_normals() + filtered.compute_vertex_normals() + + return filtered + + +def dot(a, b) -> float: + return float(a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) + + +def create_grasp_candidates_from_surfaces( + mesh: o3d.geometry.TriangleMesh, effector_type: EffectorType +) -> list[tuple[Pose, Pose]]: + """Mesh triangles -> list[(pre_grasp_pose, grasp_pose)].""" + + mesh.compute_triangle_normals() + + vertices = np.asarray(mesh.vertices) + triangles = np.asarray(mesh.triangles) + normals = np.asarray(mesh.triangle_normals) + + poses = [] + + for triangle, normal in zip(triangles, normals): + normal = normalize_vector((float(normal[0]), float(normal[1]), float(normal[2]))) + center = vertices[triangle].mean(axis=0) + orientation = orientation_from_normal(normal) + + grasp_pose = Pose( + Position( + float(center[0] + normal[0] * GRASP_OFFSET), + float(center[1] + normal[1] * GRASP_OFFSET), + float(center[2] + normal[2] * GRASP_OFFSET), + ), + orientation, + ) + pre_grasp_pose = Pose( + Position( + float(center[0] + normal[0] * PRE_GRASP_OFFSET), + float(center[1] + normal[1] * PRE_GRASP_OFFSET), + float(center[2] + normal[2] * PRE_GRASP_OFFSET), + ), + orientation, + ) + + poses.append((pre_grasp_pose, grasp_pose)) + + return poses + + +def orientation_from_normal(normal: tuple[float, float, float]) -> Orientation: + """Surface normal -> TCP orientation.""" + + nx, ny, nz = normalize_vector(normal) + z_axis = (-nx, -ny, -nz) + + helper = (0.0, 0.0, 1.0) + if abs(dot(z_axis, helper)) > 0.95: + helper = (0.0, 1.0, 0.0) + + x_axis = normalize_vector(cross(helper, z_axis)) + y_axis = cross(z_axis, x_axis) + + return quaternion_from_axes(x_axis, y_axis, z_axis) + + +def quaternion_from_axes( + x_axis: tuple[float, float, float], y_axis: tuple[float, float, float], z_axis: tuple[float, float, float] +) -> Orientation: + """TCP axes -> quaternion.""" + + m00, m01, m02 = x_axis[0], y_axis[0], z_axis[0] + m10, m11, m12 = x_axis[1], y_axis[1], z_axis[1] + m20, m21, m22 = x_axis[2], y_axis[2], z_axis[2] + + trace = m00 + m11 + m22 + + if trace > 0.0: + s = math.sqrt(trace + 1.0) * 2.0 + x, y, z, w = (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s, 0.25 * s + elif m00 > m11 and m00 > m22: + s = math.sqrt(1.0 + m00 - m11 - m22) * 2.0 + x, y, z, w = 0.25 * s, (m01 + m10) / s, (m02 + m20) / s, (m21 - m12) / s + elif m11 > m22: + s = math.sqrt(1.0 + m11 - m00 - m22) * 2.0 + x, y, z, w = (m01 + m10) / s, 0.25 * s, (m12 + m21) / s, (m02 - m20) / s + else: + s = math.sqrt(1.0 + m22 - m00 - m11) * 2.0 + x, y, z, w = (m02 + m20) / s, (m12 + m21) / s, 0.25 * s, (m10 - m01) / s + + return normalize_orientation(Orientation(x, y, z, w)) + + +def normalize_vector(v: tuple[float, float, float]) -> tuple[float, float, float]: + norm = math.sqrt(dot(v, v)) + if norm == 0.0: + raise ValueError("Invalid zero vector.") + return (v[0] / norm, v[1] / norm, v[2] / norm) + + +def normalize_orientation(q: Orientation) -> Orientation: + norm = math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) + if norm == 0.0: + raise ValueError("Invalid quaternion.") + return Orientation(q.x / norm, q.y / norm, q.z / norm, q.w / norm) + + +def cross(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]: + return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]) diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index e8385948..90c5c8e3 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -1,20 +1,31 @@ +import copy import importlib import multiprocessing +import tempfile import threading import time from multiprocessing.connection import Connection +from pathlib import Path from typing import Any, Callable, cast +import open3d as o3d import rclpy # pants: no-infer-dep +from geometry_msgs.msg import Point # pants: no-infer-dep from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep -from geometry_msgs.msg import PoseStamped # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep -from moveit_msgs.msg import AttachedCollisionObject, CollisionObject # pants: no-infer-dep +from moveit_msgs.msg import ( # pants: no-infer-dep + AttachedCollisionObject, + CollisionObject, + Constraints, + OrientationConstraint, + PositionConstraint, +) from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep from sensor_msgs.msg import JointState # pants: no-infer-dep -from shape_msgs.msg import SolidPrimitive # pants: no-infer-dep +from shape_msgs.msg import Mesh as RosMesh # pants: no-infer-dep +from shape_msgs.msg import MeshTriangle, SolidPrimitive # pants: no-infer-dep from std_msgs.msg import Bool, String # pants: no-infer-dep from std_srvs.srv import Trigger # pants: no-infer-dep from tf2_ros.buffer import Buffer # pants: no-infer-dep @@ -34,6 +45,7 @@ from arcor2_ur.common import CollisionSceneObject from arcor2_ur.exceptions import UrGeneral from arcor2_ur.object_types.ur5e import Vacuum +from arcor2_ur.scripts.grasp_planner import generate_grasp_poses from arcor2_ur.vgc10 import VGC10 logger = get_logger(__name__) @@ -55,6 +67,82 @@ def pose_to_ros_pose(ps: Pose) -> RosPose: return rp +def mesh_file_to_ros_mesh(mesh_data: bytes, asset_id: str) -> RosMesh: + if not mesh_data: + raise UrGeneral(f"Mesh asset '{asset_id}' is empty.") + + suffix = Path(asset_id).suffix.lower() + if not suffix: + raise UrGeneral(f"Mesh asset '{asset_id}' has no file extension.") + + with tempfile.NamedTemporaryFile(suffix=suffix, delete=True) as tmp: + tmp.write(mesh_data) + tmp.flush() + + loaded = o3d.io.read_triangle_mesh(tmp.name) + + if loaded.is_empty(): + raise UrGeneral(f"Mesh asset '{asset_id}' has no geometry.") + + vertices = tuple((float(vertex[0]), float(vertex[1]), float(vertex[2])) for vertex in loaded.vertices) + triangles = tuple((int(triangle[0]), int(triangle[1]), int(triangle[2])) for triangle in loaded.triangles) + + if not vertices or not triangles: + raise UrGeneral(f"Mesh asset '{asset_id}' has no usable mesh data.") + + ros_mesh = RosMesh() + + for x, y, z in vertices: + point = Point() + point.x = x + point.y = y + point.z = z + ros_mesh.vertices.append(point) + + for a, b, c in triangles: + triangle = MeshTriangle() + triangle.vertex_indices = [a, b, c] + ros_mesh.triangles.append(triangle) + + return ros_mesh + + +def make_pose_constraints( + pose: Pose, + frame_id: str, + link_name: str, + position_tolerance: float = 0.01, + orientation_tolerance: float = 0.01, +) -> Constraints: + constraints = Constraints() + + position_constraint = PositionConstraint() + position_constraint.header.frame_id = frame_id + position_constraint.link_name = link_name + position_constraint.weight = 1.0 + + sphere = SolidPrimitive() + sphere.type = SolidPrimitive.SPHERE + sphere.dimensions = [position_tolerance] + + position_constraint.constraint_region.primitives.append(sphere) + position_constraint.constraint_region.primitive_poses.append(pose_to_ros_pose(pose)) + + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = frame_id + orientation_constraint.link_name = link_name + orientation_constraint.orientation = pose_to_ros_pose(pose).orientation + orientation_constraint.absolute_x_axis_tolerance = orientation_tolerance + orientation_constraint.absolute_y_axis_tolerance = orientation_tolerance + orientation_constraint.absolute_z_axis_tolerance = orientation_tolerance + orientation_constraint.weight = 1.0 + + constraints.position_constraints.append(position_constraint) + constraints.orientation_constraints.append(orientation_constraint) + + return constraints + + def plan_and_execute( robot, planning_component, @@ -93,60 +181,6 @@ def wait_for_future(future, *, timeout_sec: float = 2.0): return future.result() -# remove id ? -def create_collision_object( - obj: CollisionSceneObject, - obj_id: str, - frame_id: str, - pose_in_frame: Pose, - attached_to_link: str | None = None, -) -> CollisionObject | AttachedCollisionObject: - - collision_object = CollisionObject() - collision_object.header.frame_id = frame_id - collision_object.id = obj_id - collision_object.operation = CollisionObject.ADD - - if isinstance(obj.model, object_type.Box): - prim = SolidPrimitive() - prim.type = SolidPrimitive.BOX - prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] - - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) - - elif isinstance(obj.model, object_type.Sphere): - prim = SolidPrimitive() - prim.type = SolidPrimitive.SPHERE - prim.dimensions = [obj.model.radius] - - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) - - elif isinstance(obj.model, object_type.Cylinder): - prim = SolidPrimitive() - prim.type = SolidPrimitive.CYLINDER - prim.dimensions = [obj.model.height, obj.model.radius] - - collision_object.primitives.append(prim) - collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) - - # mal by fungovat subor - elif isinstance(obj.model, object_type.Mesh): - mesh = storage_client.get_asset_data(obj.model.asset_id) - - collision_object.meshes.append(mesh) - collision_object.mesh_poses.append(pose_to_ros_pose(pose_in_frame)) - - if attached_to_link is not None: - attached = AttachedCollisionObject() - attached.link_name = attached_to_link - attached.object = collision_object - return attached - - return collision_object - - def rotate_vector(q: Orientation, v: tuple[float, float, float]) -> tuple[float, float, float]: x, y, z, w = q.x, q.y, q.z, q.w vx, vy, vz = v @@ -164,116 +198,6 @@ def rotate_vector(q: Orientation, v: tuple[float, float, float]) -> tuple[float, return rx, ry, rz -def rotate_effector(a: Orientation, b: Orientation) -> Orientation: - return Orientation( - a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, - a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, - a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, - a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, - ) - - -def generate_grasp_poses( - object: CollisionSceneObject, - effector_type: EffectorType, - grasp_positions: list[str] | None, -) -> list[tuple[Pose, Pose]]: - if grasp_positions is None: - grasp_positions = [GraspPosition.ALL.value] - - grasp_poses: list[tuple[Pose, Pose]] = [] - - pre_grasp_offset = 0.20 - grasp_offset = -0.01 # TODO: -0.01 ? - - if effector_type == EffectorType.SUCK: - if not isinstance(object.model, object_type.Mesh): - half_x = 0.0 - half_y = 0.0 - half_z = 0.0 - - if isinstance(object.model, object_type.Box): - half_x = object.model.size_x / 2 - half_y = object.model.size_y / 2 - half_z = object.model.size_z / 2 - - elif isinstance(object.model, object_type.Cylinder): - half_y = half_x = object.model.radius - half_z = object.model.height / 2 - - elif isinstance(object.model, object_type.Sphere): - half_z = half_y = half_x = object.model.radius - - cx = object.pose.position.x - cy = object.pose.position.y - cz = object.pose.position.z - q = object.pose.orientation - - faces = [] - if GraspPosition.ALL in grasp_positions: - faces = [ - ((0.0, 0.0, half_z), (0.0, 0.0, 1.0), Orientation(1, 0, 0, 0)), # TOP - ((-half_x, 0.0, 0.0), (-1.0, 0.0, 0.0), Orientation(0, 1, 0, 1)), # LEFT - ((half_x, 0.0, 0.0), (1.0, 0.0, 0.0), Orientation(0, -1, 0, 1)), # RIGHT - ((0.0, -half_y, 0.0), (0.0, -1.0, 0.0), Orientation(-1, 0, 0, 1)), # FRONT - ((0.0, half_y, 0.0), (0.0, 1.0, 0.0), Orientation(1, 0, 0, 1)), # BACK - ((0.0, 0.0, -half_z), (0.0, 0.0, -1.0), Orientation(0, 0, 1, 1)), # BOTTOM - ] - else: - if GraspPosition.TOP in grasp_positions: - faces.append(((0.0, 0.0, half_z), (0.0, 0.0, 1.0), Orientation(1, 0, 0, 0))) - - if GraspPosition.LEFT in grasp_positions: - faces.append(((-half_x, 0.0, 0.0), (-1.0, 0.0, 0.0), Orientation(0, 1, 0, 1))) - - if GraspPosition.RIGHT in grasp_positions: - faces.append(((half_x, 0.0, 0.0), (1.0, 0.0, 0.0), Orientation(0, -1, 0, 1))) - - if GraspPosition.FRONT in grasp_positions: - faces.append(((0.0, -half_y, 0.0), (0.0, -1.0, 0.0), Orientation(-1, 0, 0, 1))) - - if GraspPosition.BACK in grasp_positions: - faces.append(((0.0, half_y, 0.0), (0.0, 1.0, 0.0), Orientation(1, 0, 0, 1))) - - if GraspPosition.BOTTOM in grasp_positions: - faces.append(((0.0, 0.0, -half_z), (0.0, 0.0, -1.0), Orientation(0, 0, 0, 1))) - - for local_center, local_normal, local_effector_orientation in faces: - rcx, rcy, rcz = rotate_vector(q, local_center) - rnx, rny, rnz = rotate_vector(q, local_normal) - effector_orientation = rotate_effector(q, local_effector_orientation) - - face_x = cx + rcx - face_y = cy + rcy - face_z = cz + rcz - - grasp_pose = Pose( - Position( - face_x + rnx * grasp_offset, - face_y + rny * grasp_offset, - face_z + rnz * grasp_offset, - ), - effector_orientation, - ) - - pre_grasp_pose = Pose( - Position( - face_x + rnx * pre_grasp_offset, - face_y + rny * pre_grasp_offset, - face_z + rnz * pre_grasp_offset, - ), - effector_orientation, - ) - - grasp_poses.append((pre_grasp_pose, grasp_pose)) - else: # load for mesh - pass - else: - pass # other type of effector - - return grasp_poses - - def normalize_orientation(q: Orientation) -> Orientation: norm = (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) ** 0.5 if norm == 0.0: @@ -519,6 +443,7 @@ def __init__( self.base_link = base_link self.tool_link = tool_link self.collision_objects = collision_objects.copy() + self.mesh_models: dict[str, RosMesh] = {} self.joints: list[Joint] = [] self.tool: VGC10 | None = None self.freedrive_mode = False @@ -637,7 +562,7 @@ def _active_controllers(self) -> set[str]: return set() list_controllers_cls: Any = self._list_controllers_client.srv_type req = list_controllers_cls.Request() - resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=5.0) + resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) if resp is None: return set() return {c.name for c in resp.controller if c.state == "active"} @@ -670,22 +595,33 @@ def _switch_freedrive_controller(self, enable: bool) -> None: req.deactivate_controllers = to_deactivate req.strictness = 2 # STRICT req.activate_asap = False - req.timeout.sec = 5 + req.timeout.sec = 15 req.timeout.nanosec = 0 - resp = wait_for_future(self._switch_controller_client.call_async(req), timeout_sec=10.0) + resp = wait_for_future(self._switch_controller_client.call_async(req), timeout_sec=20.0) if resp is None or not resp.ok: msg = getattr(resp, "message", "") raise UrGeneral(f"Controller switch failed. {msg}") def _current_pose(self) -> Pose: ros_time = rclpy.time.Time() + deadline = time.monotonic() + 30.0 + last_error: Exception | None = None - if not self.node.buffer.can_transform( - self.base_link, self.tool_link, ros_time, timeout=rclpy.time.Duration(seconds=5.0) - ): - raise UrGeneral("Can't get transform") - transform = self.node.buffer.lookup_transform(self.base_link, self.tool_link, ros_time) + while time.monotonic() < deadline: + try: + transform = self.node.buffer.lookup_transform( + self.base_link, + self.tool_link, + ros_time, + timeout=rclpy.time.Duration(seconds=1.0), + ) + break + except Exception as exc: + last_error = exc + time.sleep(0.05) + else: + raise UrGeneral(f"Can't get transform {self.base_link}->{self.tool_link}: {last_error}") t = transform.transform @@ -751,7 +687,13 @@ def _wait_for_pose_reached(self, target: Pose, timeout: float = 15.0, tolerance: time.sleep(0.02) raise UrGeneral("Target pose was not reached.") - def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) -> dict: + def move_to_pose( + self, + pose: Pose, + velocity: float, + payload: float, + safe: bool, + ) -> dict: if self.freedrive_mode: raise UrGeneral("Freedrive mode is active. Disable hand teaching before commanding motion.") @@ -760,17 +702,6 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) previous_joints = list(self.joints) pose = tr.make_pose_rel(self.base_pose, pose) - pose_goal = PoseStamped() - pose_goal.header.frame_id = self.base_link - - pose_goal.pose.orientation.x = pose.orientation.x - pose_goal.pose.orientation.y = pose.orientation.y - pose_goal.pose.orientation.z = pose.orientation.z - pose_goal.pose.orientation.w = pose.orientation.w - pose_goal.pose.position.x = pose.position.x - pose_goal.pose.position.y = pose.position.y - pose_goal.pose.position.z = pose.position.z - moveitpy, ur_manipulator = self._get_moveit() with moveitpy.get_planning_scene_monitor().read_write() as scene: if safe: @@ -783,7 +714,14 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) scene.current_state.update() ur_manipulator.set_start_state_to_current_state() - ur_manipulator.set_goal_state(pose_stamped_msg=pose_goal, pose_link=self.tool_link) + goal_constraints = make_pose_constraints( + pose, + self.base_link, + self.tool_link, + position_tolerance=0.01, + orientation_tolerance=0.01, + ) + ur_manipulator.set_goal_state(motion_plan_constraints=[goal_constraints]) ur_manipulator.set_workspace(*WORKSPACE_MIN, *WORKSPACE_MAX) # not documented anywhere :-D self.node.set_speed_slider(velocity) @@ -793,44 +731,157 @@ def move_to_pose(self, pose: Pose, velocity: float, payload: float, safe: bool) # TODO why is this needed (for tests to pass)? self._wait_for_joint_update(previous_joints) - self._wait_for_transform_update(previous_transform_time) - self._wait_for_pose_reached(target_abs) + try: + self._wait_for_transform_update(previous_transform_time, timeout=15.0) + except UrGeneral: + logger.warning("Transform did not update after motion, checking pose anyway.") + self._wait_for_pose_reached(target_abs, tolerance=0.01) return {} def apply_collision_objects(self, scene) -> None: scene.remove_all_collision_objects() + scene.current_state.update(force=True) + attached_objects = scene.planning_scene_message.robot_state.attached_collision_objects + attached_id = attached_objects[0].object.id if attached_objects else None + for obj_id, obj in self.collision_objects.items(): state = obj.metadata.get("state") - if state == GraspableState.LOST: + if state == GraspableState.HIDDEN: continue - if state == GraspableState.HIDDEN: + # object is already attached + elif state == GraspableState.ATTACHED and attached_id is not None: continue - # TODO: choose it by id - if state == GraspableState.ATTACHED: + elif state == GraspableState.ATTACHED: attached_pose_dict = obj.metadata.get("attached_pose") if attached_pose_dict is None: - logger.warning(f"Attached object {obj_id} is missing attached_pose. Skipping.") - continue + raise UrGeneral(f"Attached object {obj_id} has no attached_pose metadata.") + + pose_in_frame = Pose.from_dict(cast(dict[str, Any], attached_pose_dict)) - pose_in_frame = Pose.from_dict(attached_pose_dict) - attached = create_collision_object(obj, obj_id, self.tool_link, pose_in_frame, self.tool_link) - if attached: - scene.process_attached_collision_object(attached) + attached = self.create_collision_object(obj, obj_id, self.tool_link, pose_in_frame, self.tool_link) + scene.process_attached_collision_object(attached) + logger.info(f"Attached object id: {obj_id}") continue - # normal collision object - pose_in_frame = tr.make_pose_rel(self.base_pose, obj.pose) - collision = create_collision_object(obj, obj_id, self.base_link, pose_in_frame) - if collision: + else: # normal collision object + pose_in_frame = tr.make_pose_rel(self.base_pose, obj.pose) + collision = self.create_collision_object(obj, obj_id, self.base_link, pose_in_frame) scene.apply_collision_object(collision) + if state == GraspableState.TOUCH_ALLOWED: + self.allow_collision_between_object_and_links( + scene, obj_id, [self.tool_link, "suction_base_link", "suction_cup_link", "suction_tcp"] + ) # TODO auto + scene.current_state.update(force=True) scene.current_state.update() + # remove id ? + def create_collision_object( + self, + obj: CollisionSceneObject, + obj_id: str, + frame_id: str, + pose_in_frame: Pose, + attached_to_link: str | None = None, + ) -> CollisionObject | AttachedCollisionObject: + + collision_object = CollisionObject() + collision_object.header.frame_id = frame_id + collision_object.id = obj_id + collision_object.operation = CollisionObject.ADD + + if isinstance(obj.model, object_type.Box): + prim = SolidPrimitive() + prim.type = SolidPrimitive.BOX + prim.dimensions = [obj.model.size_x, obj.model.size_y, obj.model.size_z] + + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) + + elif isinstance(obj.model, object_type.Sphere): + prim = SolidPrimitive() + prim.type = SolidPrimitive.SPHERE + prim.dimensions = [obj.model.radius] + + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) + + elif isinstance(obj.model, object_type.Cylinder): + prim = SolidPrimitive() + prim.type = SolidPrimitive.CYLINDER + prim.dimensions = [obj.model.height, obj.model.radius] + + collision_object.primitives.append(prim) + collision_object.primitive_poses.append(pose_to_ros_pose(pose_in_frame)) + + elif isinstance(obj.model, object_type.Mesh): + asset_id = obj.model.asset_id + cached_mesh = self.mesh_models.get(asset_id) + + if cached_mesh is None: + mesh_data = storage_client.get_asset_data(asset_id) + cached_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id) + self.mesh_models[asset_id] = cached_mesh + + collision_object.meshes.append(copy.deepcopy(cached_mesh)) + collision_object.mesh_poses.append(pose_to_ros_pose(pose_in_frame)) + + if attached_to_link is not None: + attached = AttachedCollisionObject() + attached.link_name = attached_to_link + attached.object = collision_object + attached.touch_links = [attached_to_link, "suction_base_link", "suction_cup_link"] + return attached + + return collision_object + + def allow_collision_between_object_and_links(self, scene, object_id: str, link_names: list[str]) -> None: + acm = scene.planning_scene_message.allowed_collision_matrix + + names = list(acm.entry_names) + values = list(acm.entry_values) + + for name in [object_id, *link_names]: + if name not in names: + names.append(name) + for row in values: + row.enabled.append(False) + + new_row = copy.deepcopy(values[0]) if values else type(acm.entry_values[0])() + new_row.enabled = [False] * len(names) + values.append(new_row) + + for link_name in link_names: + i = names.index(object_id) + j = names.index(link_name) + values[i].enabled[j] = True + values[j].enabled[i] = True + + acm.entry_names = names + acm.entry_values = values + + def filter_grasps_by_ik(self, grasp_options: list[tuple[Pose, Pose]]) -> list[tuple[Pose, Pose]]: + valid = [] + + for pre_grasp_pose, grasp_pose in grasp_options: + try: + self.inverse_kinematics( + InverseKinematicsRequest(pose=pre_grasp_pose, start_joints=self.joints, avoid_collisions=True) + ) + self.inverse_kinematics( + InverseKinematicsRequest(pose=grasp_pose, start_joints=self.joints, avoid_collisions=False) + ) + valid.append((pre_grasp_pose, grasp_pose)) + except Exception: + continue + + return valid + def get_joints(self) -> list[dict]: return [joint.to_dict() for joint in self.joints] @@ -917,17 +968,11 @@ def _get_moveit(self) -> tuple[MoveItPy, PlanningComponent]: raise UrGeneral("MoveIt is not initialized.") return self.moveitpy, self.ur_manipulator - def get_attached_object_id(self) -> str | None: - for obj_id, obj in self.collision_objects.items(): - if obj.metadata.get("state") == GraspableState.ATTACHED: - return obj_id - return None - def attach_object( self, object_id: str, effector_type: EffectorType, - grasp_positions: list[str] | None, + grasp_positions: list[GraspPosition] | None, velocity: float, payload: float, safe: bool, @@ -935,17 +980,33 @@ def attach_object( object = self.collision_objects[object_id] object.metadata.pop("attached_pose", None) - grasp_options = generate_grasp_poses(object, effector_type, grasp_positions) - for pre_grasp_pose, grasp_pose in grasp_options: + ros_mesh = None + if isinstance(object.model, object_type.Mesh): + asset_id = object.model.asset_id + ros_mesh = self.mesh_models.get(asset_id) + + if ros_mesh is None: + mesh_data = storage_client.get_asset_data(asset_id) + ros_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id) + self.mesh_models[asset_id] = ros_mesh + + grasp_options = generate_grasp_poses(object, effector_type, grasp_positions, ros_mesh) + logger.info(f"Generated {len(grasp_options)} grasp options for object {object_id}.") + + grasp_options = self.filter_grasps_by_ik(grasp_options) + logger.info(f"IK accepted: {len(grasp_options)} grasp options.") + for pre_grasp_pose, grasp_pose in grasp_options: try: self.move_to_pose(pre_grasp_pose, velocity, payload, safe) - object.metadata["state"] = GraspableState.HIDDEN - if self.tool: # Skip vacuum activation/check when no physical tool is configured + if self.tool: self.suck(60) time.sleep(1.0) + object.metadata["state"] = GraspableState.TOUCH_ALLOWED + self.update_collisions(self.collision_objects) + self.move_to_pose(grasp_pose, velocity, payload, safe) if self.tool: @@ -958,6 +1019,8 @@ def attach_object( attached_pose = tr.make_pose_rel(grasp_pose, object.pose) object.metadata["attached_pose"] = attached_pose.to_dict() + self.update_collisions(self.collision_objects) + except Exception: continue @@ -965,11 +1028,18 @@ def attach_object( raise UrGeneral(f"Unable to attach object {object_id}.") - def detach_object(self, target_pose: Pose, velocity: float, payload: float, safe: bool) -> dict[str, Any]: + def detach_object( + self, + target_pose: Pose, + velocity: float, + payload: float, + safe: bool, + ) -> dict[str, Any]: object_id = self.get_attached_object_id() if object_id is None: raise UrGeneral("No object to detach.") + logger.info(f"Detaching object id: {object_id}") object = self.collision_objects[object_id] attached_pose_dict = object.metadata.get("attached_pose") @@ -982,30 +1052,52 @@ def detach_object(self, target_pose: Pose, velocity: float, payload: float, safe self.move_to_pose(target_eef_pose, velocity, payload, safe) - self.release() - - self.remove_object(object_id) + if self.tool: + self.release() + time.sleep(1.0) - object.metadata["state"] = GraspableState.HIDDEN + object.metadata["state"] = GraspableState.TOUCH_ALLOWED + self.remove_attached_object(object_id) object.metadata.pop("attached_pose", None) - self.update_collisions(self.collision_objects) object.pose = target_pose + self.update_collisions(self.collision_objects) self.move_to_pose(post_detach_pose, velocity, payload, safe) object.metadata["state"] = GraspableState.WORLD self.update_collisions(self.collision_objects) + logger.info(f"Detached object id: {object_id}") + return {"object_id": object_id, "state": object.metadata["state"]} - def remove_object(self, object_id: str) -> None: + def get_attached_object_id(self) -> str | None: + + moveitpy, _ = self._get_moveit() + + planning_scene_msg = None + with moveitpy.get_planning_scene_monitor().read_write() as scene: + scene.current_state.update(force=True) + planning_scene_msg = scene.planning_scene_message + + attached_objects = planning_scene_msg.robot_state.attached_collision_objects + + if not attached_objects: + return None + + return attached_objects[0].object.id + + def remove_attached_object(self, object_id: str) -> None: moveitpy, _ = self._get_moveit() - co = CollisionObject() - co.id = object_id - co.operation = CollisionObject.REMOVE + + attached = AttachedCollisionObject() + attached.link_name = self.tool_link + attached.object.id = object_id + attached.object.operation = CollisionObject.REMOVE with moveitpy.get_planning_scene_monitor().read_write() as scene: - scene.apply_collision_object(co) + scene.process_attached_collision_object(attached) + scene.current_state.update(force=True) def ros_worker_main( @@ -1071,7 +1163,12 @@ def ros_worker_main( result = runtime.get_eef_pose() elif op == "move_to_pose": pose = Pose.from_dict(kwargs["pose"]) - result = runtime.move_to_pose(pose, kwargs["velocity"], kwargs["payload"], kwargs["safe"]) + result = runtime.move_to_pose( + pose, + kwargs["velocity"], + kwargs["payload"], + kwargs["safe"], + ) elif op == "pick_up_object": result = runtime.attach_object( kwargs["object_id"], diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index a10b2653..c7daa11c 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -82,15 +82,18 @@ def load_custom_urdf() -> str: return text +def load_custom_srdf() -> str: + srdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.srdf") + return srdf_res.read_text(encoding="utf-8") + + moveit_yaml_res = resources.files("arcor2_ur").joinpath("data/moveit.yaml") +custom_srdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.srdf") -with resources.as_file(moveit_yaml_res) as moveit_yaml_path: +with resources.as_file(moveit_yaml_res) as moveit_yaml_path, resources.as_file(custom_srdf_res) as custom_srdf_path: moveit_config = ( MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config") - .robot_description_semantic( - os.path.join(get_package_share_directory("ur_moveit_config"), "srdf", "ur.srdf.xacro"), - {"name": UR_TYPE}, - ) + .robot_description_semantic(str(custom_srdf_path)) .trajectory_execution( os.path.join(get_package_share_directory("ur_moveit_config"), "config", "moveit_controllers.yaml") ) @@ -102,6 +105,7 @@ def load_custom_urdf() -> str: ).to_dict() moveit_config["robot_description"] = load_custom_urdf() +moveit_config["robot_description_semantic"] = load_custom_srdf() def started() -> bool: @@ -221,9 +225,21 @@ def get_started() -> RespT: return jsonify(started()) -@app.route("/graspable/pick_up_object", methods=["PUT"]) +@app.route("/graspable//state", methods=["GET"]) +def get_graspable_state(object_id: str) -> RespT: + """Return graspable object state.""" + return jsonify(globs.collision_objects[object_id].metadata["state"]) + + +@app.route("/graspable//position", methods=["GET"]) +def get_graspable_position(object_id: str) -> RespT: + """Return graspable object position.""" + return jsonify(globs.collision_objects[object_id].pose.position.to_dict()) + + +@app.route("/graspable/pick_up_object_by_position", methods=["PUT"]) @requires_started -def pick_up_object() -> RespT: +def pick_up_object_by_position() -> RespT: """Find nearest graspable object and attach it. --- put: @@ -304,8 +320,11 @@ def pick_up_object() -> RespT: ], ) - object_type_cls = getattr(object_type, body["object_type_name"]) if body.get("object_type_name") else None - + object_type_name = ( + object_type.MODEL_MAPPING[object_type.Model3dType(body["object_type_name"])] + if body.get("object_type_name") + else None + ) velocity = float(body.get("velocity", 50.0)) / 100.0 payload = float(body.get("payload", 0.0)) safe = bool(body.get("safe", True)) @@ -320,7 +339,7 @@ def pick_up_object() -> RespT: if obj.metadata.get("state") != GraspableState.WORLD: continue - if object_type_cls is not None and not isinstance(obj.model, object_type_cls): + if object_type_name is not None and not isinstance(obj.model, object_type_name): continue current_distance = position.distance(obj.pose.position) @@ -362,6 +381,102 @@ def pick_up_object() -> RespT: return Response(status=204) +@app.route("/graspable/pick_up_object_by_id", methods=["PUT"]) +@requires_started +def pick_up_object_by_id() -> RespT: + """Attach graspable object by ID. + --- + put: + tags: + - Graspable + summary: Attach graspable object by ID. + requestBody: + required: true + content: + application/json: + schema: + type: object + required: + - object_id + properties: + object_id: + type: string + effector_type: + type: string + default: suck + grasp_positions: + type: array + items: + type: string + velocity: + type: number + format: float + default: 50.0 + payload: + type: number + format: float + default: 0.0 + safe: + type: boolean + default: true + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **UrGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + body = humps.decamelize(request.json) + + object_id = body["object_id"] + effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK)) + grasp_positions = body.get( + "grasp_positions", + [ + GraspPosition.TOP, + GraspPosition.RIGHT, + GraspPosition.LEFT, + GraspPosition.FRONT, + GraspPosition.BACK, + GraspPosition.BOTTOM, + GraspPosition.ALL, + ], + ) + velocity = float(body.get("velocity", 50.0)) / 100.0 + payload = float(body.get("payload", 0.0)) + safe = bool(body.get("safe", True)) + + selected = globs.collision_objects[object_id] + previous_state = selected.metadata.get("state") + selected.metadata["state"] = GraspableState.RESERVED + + assert globs.state + + try: + globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) + metadata = globs.state.worker.request( + "pick_up_object", + object_id=object_id, + effector_type=effector_type, + grasp_positions=grasp_positions, + velocity=velocity, + payload=payload, + safe=safe, + ) + + selected.metadata.clear() + selected.metadata.update(metadata) + except Exception: + selected.metadata["state"] = previous_state + globs.state.worker.request("update_collisions", collision_objects=globs.collision_objects) + raise + + return Response(status=204) + + @app.route("/graspable/place_object", methods=["PUT"]) @requires_started def place_object() -> RespT: @@ -931,7 +1046,13 @@ def put_eef_pose() -> RespT: payload = float(request.args.get("payload", default=0.0)) safe = request.args.get("safe", default="true") == "true" - globs.state.worker.request("move_to_pose", pose=pose.to_dict(), velocity=velocity, payload=payload, safe=safe) + globs.state.worker.request( + "move_to_pose", + pose=pose.to_dict(), + velocity=velocity, + payload=payload, + safe=safe, + ) return Response(status=204) diff --git a/src/python/arcor2_ur/tests/BUILD b/src/python/arcor2_ur/tests/BUILD index 838bf73d..7e07d76e 100644 --- a/src/python/arcor2_ur/tests/BUILD +++ b/src/python/arcor2_ur/tests/BUILD @@ -1,17 +1,33 @@ +files( + name="mesh_assets", + sources=[ + "test_mesh_object/triangle_block.stl", + ], +) + +python_test_utils( + name="test_utils", + sources=["conftest.py"], +) + python_tests( name="tests", - sources=["test_*.py"], + sources=[ + "test_*.py", + "test_collision/test_*.py", + "test_pick_up/test_*.py", + "test_pick_and_place/test_*.py", + ], dependencies=[ + ":test_utils", + ":mesh_assets", "src/python/arcor2_ur/data/urdf:urdf", "src/python/arcor2_ur/data:moveit_yaml", ], runtime_package_dependencies=[ "src/python/arcor2_ur/scripts:ur", "src/python/arcor2_ur/scripts:robot_publisher", + "src/python/arcor2_storage/scripts:storage", ], -) - -python_test_utils( - name="test_utils", - sources=["conftest.py"], + tags=["unstable_ros"], ) \ No newline at end of file diff --git a/src/python/arcor2_ur/tests/conftest.py b/src/python/arcor2_ur/tests/conftest.py index 8a6f9a3b..4f50caf7 100644 --- a/src/python/arcor2_ur/tests/conftest.py +++ b/src/python/arcor2_ur/tests/conftest.py @@ -11,6 +11,7 @@ from arcor2.helpers import find_free_port from arcor2_arserver.tests.testutils import CheckHealthException, check_health, log_proc_output +from arcor2_storage import client as storage_client LOGGER = logging.getLogger(__name__) @@ -18,6 +19,7 @@ class Urls(NamedTuple): ros_domain_id: str robot_url: str + storage_url: str def _finish_processes(processes) -> None: @@ -27,6 +29,7 @@ def _finish_processes(processes) -> None: os.killpg(proc.pid, signal.SIGTERM) except ProcessLookupError: pass + try: proc.wait(timeout=5) except sp.TimeoutExpired: @@ -35,6 +38,7 @@ def _finish_processes(processes) -> None: except ProcessLookupError: pass proc.wait(timeout=1) + log_proc_output(proc.communicate()) @@ -50,34 +54,45 @@ def _load_ros_env() -> dict[str, str]: raise RuntimeError("Failed to source /opt/ros/jazzy/setup.bash") from exc env: dict[str, str] = {} + for chunk in output.split(b"\0"): if not chunk: continue + key, _, value = chunk.partition(b"=") env[key.decode()] = value.decode() + return env @pytest.fixture(scope="module", params=["ur5e"]) def start_processes(request) -> Iterator[Urls]: - """Starts Dobot dependencies.""" + """Starts UR test dependencies.""" ros_domain_id = str(random.sample(range(0, 232 + 1), 1)[0]) ur_type: str = request.param processes = [] + ros_env = _load_ros_env() my_env = os.environ.copy() my_env.update(ros_env) my_env["ROS_DOMAIN_ID"] = ros_domain_id - # Keep ROS view of the world dominant; strip test-only/debug env for the ROS launcher + # Keep ROS view of the world dominant; strip test-only/debug env for the ROS launcher. ros_launch_env = {k: v for k, v in my_env.items() if not k.startswith("PEX_")} + for key in ("PYTEST_DISABLE_PLUGIN_AUTOLOAD", "PYTEST_PLUGINS", "PYTHONDEVMODE", "PYTHONWARNINGS"): ros_launch_env.pop(key, None) - kwargs = {"env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT, "start_new_session": True} + kwargs = { + "env": my_env, + "stdout": sp.PIPE, + "stderr": sp.STDOUT, + "start_new_session": True, + } + # Start ROS control with mock hardware. processes.append( sp.Popen( [ @@ -96,15 +111,17 @@ def start_processes(request) -> Iterator[Urls]: start_new_session=True, ) ) - time.sleep(3) # TODO find another way how to make sure that everything is running + + time.sleep(3) + if processes[-1].poll(): log_proc_output(processes[-1].communicate()) - raise Exception("Launch died...") + raise Exception("Launch died.") - # kill default robot_state_publisher + # Kill default robot_state_publisher. sp.run("pkill -f robot_state_publisher", shell=True) - # start custom robot_state_publisher with modified URDF + # Start custom robot_state_publisher with modified URDF. urdf_path = Path(__file__).resolve().parents[1] / "data" / "urdf" / "ur5e.urdf" custom_rsp = sp.Popen( @@ -127,14 +144,46 @@ def start_processes(request) -> Iterator[Urls]: time.sleep(2) + # Start ARCOR2 storage service. + storage_port = find_free_port() + storage_url = f"http://127.0.0.1:{storage_port}" + + my_env["ARCOR2_STORAGE_SERVICE_PORT"] = str(storage_port) + my_env["ARCOR2_STORAGE_SERVICE_URL"] = storage_url + my_env["ARCOR2_STORAGE_DB_PATH"] = f"/tmp/arcor2_storage_{ros_domain_id}.sqlite" + + storage_client.URL = storage_url + + storage_proc = sp.Popen( + ["python", "src.python.arcor2_storage.scripts/storage.pex"], + **kwargs, + ) # type: ignore + + processes.append(storage_proc) + + if storage_proc.poll(): + _finish_processes(processes) + raise Exception("Storage service died.") + + try: + check_health("Storage", storage_url, timeout=20) + except CheckHealthException: + _finish_processes(processes) + raise + + # Start UR service. robot_url = f"http://0.0.0.0:{find_free_port()}" + my_env["ARCOR2_UR_URL"] = robot_url my_env["ARCOR2_UR_INTERACT_WITH_DASHBOARD"] = "false" my_env["ARCOR2_UR_TYPE"] = ur_type my_env["PEX_EXTRA_SYS_PATH"] = "/opt/ros/jazzy/lib/python3.12/site-packages" my_env["ARCOR2_REST_API_DEBUG"] = "true" - robot_proc = sp.Popen(["python", "src.python.arcor2_ur.scripts/ur.pex"], **kwargs) # type: ignore + robot_proc = sp.Popen( + ["python", "src.python.arcor2_ur.scripts/ur.pex"], + **kwargs, + ) # type: ignore processes.append(robot_proc) @@ -148,15 +197,18 @@ def start_processes(request) -> Iterator[Urls]: _finish_processes(processes) raise - # robot_mode etc. is not published with mock_hw -> there is this helper node to do that - # it can't be published from here as it depends on ROS (Python 3.12) - robot_pub_proc = sp.Popen(["python", "src.python.arcor2_ur.scripts/robot_publisher.pex"], **kwargs) # type: ignore + # robot_mode etc. is not published with mock_hw -> helper node publishes it. + robot_pub_proc = sp.Popen( + ["python", "src.python.arcor2_ur.scripts/robot_publisher.pex"], + **kwargs, + ) # type: ignore + processes.append(robot_pub_proc) if robot_pub_proc.poll(): _finish_processes(processes) raise Exception("Robot publisher node died.") - yield Urls(ros_domain_id, robot_url) + yield Urls(ros_domain_id, robot_url, storage_url) _finish_processes(processes) diff --git a/src/python/arcor2_ur/tests/results_velka spatna.txt b/src/python/arcor2_ur/tests/results_velka spatna.txt new file mode 100644 index 00000000..7d59d879 --- /dev/null +++ b/src/python/arcor2_ur/tests/results_velka spatna.txt @@ -0,0 +1,28886 @@ +12:01:03.71 [WARN] Pants cannot infer owners for the following imports in the target src/python/arcor2_ur/scripts/ros_worker.py: + + * moveit_msgs.msg.AttachedCollisionObject (line: 17) + * moveit_msgs.msg.CollisionObject (line: 18) + * moveit_msgs.msg.Constraints (line: 19) + * moveit_msgs.msg.OrientationConstraint (line: 20) + * moveit_msgs.msg.PositionConstraint (line: 21) + +If you do not expect an import to be inferable, add `# pants: no-infer-dep` to the import line. Otherwise, see https://www.pantsbuild.org/2.31/docs/using-pants/troubleshooting-common-issues#import-errors-and-missing-dependencies for common problems. +12:01:06.08 [INFO] Completed: Scheduling: Find interpreter for constraints: CPython==3.12.* +12:01:06.09 [INFO] Completed: Scheduling: Building local_dists.pex +12:01:06.11 [INFO] Completed: Scheduling: Building 21 requirements for src.python.arcor2_storage.scripts/storage.pex from the 3rdparty/constraints.txt resolve: Flask~=3.1.2, Pillow~=12.1.1, aiologger~=0.7.0, apispec-webframeworks~... (467 characters truncated) +12:01:06.11 [INFO] Completed: Scheduling: Building 3 requirements for src.python.arcor2_ur.scripts/robot_publisher.pex from the 3rdparty/constraints.txt resolve: PyYAML~=6.0.3, numpy~=2.3.5, open3d==0.19.0 +12:01:06.11 [INFO] Completed: Scheduling: Building 26 requirements for src.python.arcor2_ur.scripts/ur.pex from the 3rdparty/constraints.txt resolve: Flask~=3.1.2, Pillow~=12.1.1, PyYAML~=6.0.3, aiologger~=0.7.0, apispec-webframew... (535 characters truncated) +12:01:06.39 [INFO] Completed: Scheduling: Building 17 requirements for requirements.pex from the 3rdparty/constraints.txt resolve: Pillow~=12.1.1, aiologger~=0.7.0, colorlog~=6.10.1, dataclasses-jsonschema[apispec,fast-dateparsing... (329 characters truncated) +12:01:06.70 [INFO] Completed: Scheduling: Building pytest.pex +12:01:06.71 [INFO] Completed: Scheduling: Building 20 requirements for requirements.pex from the 3rdparty/constraints.txt resolve: Pillow~=12.1.1, PyYAML~=6.0.3, aiologger~=0.7.0, colorlog~=6.10.1, dataclasses-jsonschema[apispec,f... (406 characters truncated) +12:01:06.72 [INFO] Completed: Scheduling: Building 19 requirements for requirements.pex from the 3rdparty/constraints.txt resolve: Pillow~=12.1.1, PyYAML~=6.0.3, aiologger~=0.7.0, colorlog~=6.10.1, dataclasses-jsonschema[apispec,f... (360 characters truncated) +12:01:06.83 [INFO] Completed: Scheduling: Test binary /bin/bash. +12:01:06.83 [INFO] Completed: Scheduling: Test binary /usr/bin/bash. +12:01:06.85 [INFO] Completed: Scheduling: Building pytest_runner.pex +12:01:06.85 [INFO] Completed: Scheduling: Building pytest_runner.pex +12:01:06.85 [INFO] Completed: Scheduling: Building pytest_runner.pex +12:01:06.89 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_interaction.py +12:01:06.91 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_interaction.py - succeeded. +12:01:18.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py +12:01:48.37 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests +12:01:53.37 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests +12:02:02.95 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests +12:02:27.81 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests +12:02:32.25 [INFO] Long running tasks: + 85.38s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:02:39.49 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests +12:03:02.32 [INFO] Long running tasks: + 73.95s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests + 115.45s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:03:25.42 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests +12:03:32.36 [INFO] Long running tasks: + 64.56s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 103.99s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests + 145.49s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:03:57.17 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:04:02.42 [INFO] Long running tasks: + 94.61s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 175.55s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:04:23.86 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests +12:04:32.46 [INFO] Long running tasks: + 124.66s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 205.59s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:04:39.75 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:04:43.98 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests +12:05:02.51 [INFO] Long running tasks: + 154.70s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:05:12.94 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests +12:05:32.55 [INFO] Long running tasks: + 68.69s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests + 184.74s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:05:46.51 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests +12:05:51.64 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests +12:05:57.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:06:02.61 [INFO] Long running tasks: + 98.76s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests +12:06:23.33 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests +12:06:32.65 [INFO] Long running tasks: + 128.79s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests +12:06:32.76 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests +12:07:02.68 [INFO] Long running tasks: + 65.32s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 76.17s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:07:07.26 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py:../tests +12:07:07.27 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py:../tests - succeeded. +12:07:09.66 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests +12:07:32.74 [INFO] Long running tasks: + 95.38s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 106.23s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:07:53.35 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests +12:08:02.78 [INFO] Long running tasks: + 125.42s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 136.27s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:08:30.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests +12:08:32.81 [INFO] Long running tasks: + 155.46s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 166.31s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:08:45.68 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests +12:08:49.17 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_ur.py +12:08:49.17 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_ur.py - succeeded. +12:09:02.86 [INFO] Long running tasks: + 185.50s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 196.35s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:09:02.96 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests +12:09:05.06 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:09:15.78 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py +12:09:19.20 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:09:21.58 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests +12:09:35.71 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests +12:09:49.56 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests +12:10:03.41 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests +12:10:08.45 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests +12:10:14.66 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests +12:10:33.20 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests +12:10:48.75 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests +12:11:03.91 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests +12:11:33.10 [INFO] Long running tasks: + 84.65s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:11:37.41 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests +12:11:46.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests +12:12:03.16 [INFO] Long running tasks: + 89.96s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 114.72s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:12:11.16 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests +12:12:17.75 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:12:33.22 [INFO] Long running tasks: + 120.02s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:12:50.45 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests +12:12:56.56 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests +12:13:03.26 [INFO] Long running tasks: + 150.06s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:13:33.30 [INFO] Long running tasks: + 82.14s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 180.09s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:13:51.60 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:13:54.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests +12:14:03.33 [INFO] Long running tasks: + 72.89s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests + 112.18s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:14:33.38 [INFO] Long running tasks: + 102.93s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests + 142.22s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:14:34.48 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests +12:14:35.49 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests +12:14:41.62 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests +12:15:03.44 [INFO] Long running tasks: + 172.29s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:15:09.77 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests +12:15:20.55 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py +12:15:33.49 [INFO] Long running tasks: + 202.33s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:15:45.14 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:15:57.09 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests +12:16:03.53 [INFO] Long running tasks: + 81.91s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:16:18.18 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests +12:16:31.93 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests +12:16:33.58 [INFO] Long running tasks: + 73.03s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 111.96s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:16:52.27 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests +12:17:03.63 [INFO] Long running tasks: + 103.08s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 142.00s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:17:26.65 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests +12:17:33.65 [INFO] Long running tasks: + 61.72s Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests + 133.10s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 172.03s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:17:34.01 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests +12:18:03.69 [INFO] Long running tasks: + 163.15s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 202.07s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:18:09.73 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests +12:18:12.87 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:18:33.74 [INFO] Long running tasks: + 193.19s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:18:35.78 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:18:39.06 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests +12:19:06.22 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests +12:19:09.29 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests +12:19:14.24 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests +12:19:14.24 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests - succeeded. +12:19:31.72 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests +12:19:38.74 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests +12:19:48.01 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests +12:20:12.90 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests +12:20:33.94 [INFO] Long running tasks: + 84.65s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:20:45.67 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests +12:21:01.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests +12:21:03.98 [INFO] Long running tasks: + 85.24s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 114.69s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:21:23.51 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:21:34.03 [INFO] Long running tasks: + 115.29s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:21:42.12 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests +12:21:51.81 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py +12:21:56.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests +12:21:58.94 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests +12:22:04.08 [INFO] Long running tasks: + 145.34s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:22:10.12 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:22:40.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests +12:22:44.90 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests +12:23:04.17 [INFO] Long running tasks: + 67.80s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests + 72.36s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:23:12.47 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests +12:23:17.53 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests +12:23:21.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests +12:23:34.25 [INFO] Long running tasks: + 102.44s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:23:57.42 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests +12:24:03.53 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests +12:24:04.31 [INFO] Long running tasks: + 132.50s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:24:34.37 [INFO] Long running tasks: + 73.35s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 162.56s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:24:49.65 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests +12:25:04.41 [INFO] Long running tasks: + 66.99s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 103.39s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 192.60s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:25:25.49 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:25:34.45 [INFO] Long running tasks: + 97.03s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 133.43s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:25:37.62 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests +12:25:59.88 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests +12:26:04.51 [INFO] Long running tasks: + 127.09s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 163.49s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:26:25.37 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests +12:26:33.52 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:26:34.55 [INFO] Long running tasks: + 157.13s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:26:36.55 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests +12:26:59.16 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests +12:27:04.61 [INFO] Long running tasks: + 187.19s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:27:32.96 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:27:34.67 [INFO] Long running tasks: + 61.14s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests +12:27:48.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests +12:27:55.34 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests +12:28:04.70 [INFO] Long running tasks: + 91.18s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests +12:28:06.22 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py +12:28:06.23 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_suck_effector.py - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-Tx0YCO +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_suck_effector.py::test_suck_effector[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:28:05 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-27-56-910827-DESKTOP-HG3Q5KV-27852 (testutils.py:33) + +2026-05-10 12:28:05 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [27859] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [27860] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [27861] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-4]: process started with pid [27862] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-5]: process started with pid [27863] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.262355366] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.265306507] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778408877.265547580] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.268420450] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.268437803] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.268441811] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408877.268543033] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.437860768] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.440923939] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.441738537] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.441850419] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.450163280] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451553482] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451584381] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451636148] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451649484] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451739815] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451811982] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.565168373] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.817227969] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.817293463] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.820292879] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.844506788] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.845677854] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.845936956] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.846002861] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.846033409] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.850651427] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.857973365] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.858015245] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.859064499] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.872216249] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.872494898] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.873314225] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.876105895] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.876142073] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.876883507] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.889846970] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.890133063] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.890659924] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.892238685] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.892256408] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.892827714] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.904033225] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.904279914] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.904715772] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.906323036] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.906359135] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.910187540] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.923775162] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.924053687] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.932449359] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.932485348] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.932742026] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.945770801] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.946032684] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.952046932] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.952086958] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.952462411] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.966332532] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.966625964] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.970618645] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.970656067] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.971128484] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.983880325] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.984156811] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.067755179] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 27863] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.320264589] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.320330243] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.321752576] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.336358699] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.337372075] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.337451245] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.342376314] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.343061296] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.346025742] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.346938773] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.346973479] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.347325428] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.362036285] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.362372483] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.363680574] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408878.374293431] [controller_manager]: Overrun might occur, Total time : 9448.083 us (Expected < 2000.000 us) --> Read time : 12.313 us, Update time : 9432.433 us (Switch time : 9384.542 us (Switch chained mode time : 0.662 us, perform mode change time : 10.710 us, Activation time : 9358.082 us, Deactivation time : 0.160 us)), Write time : 3.337 us (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408878.374356510] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.586517 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.374307067] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.376164455] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.377239573] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.377271183] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.377704972] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.390096869] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.405246826] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.406319725] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.406390429] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.410969053] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.412893871] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.417552378] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.419227010] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.419244694] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.421780647] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.450187558] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.453977926] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.462150650] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.464234079] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.465000675] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.468158520] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.470198538] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.470217594] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.470656363] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.488204049] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.488568136] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.491711058] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.492998349] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.495978237] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.497369735] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.497405874] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.497744688] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.510989105] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.528010061] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528574694] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528749977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528765727] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528775596] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.531864716] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.554122152] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.555398052] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.570292217] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 27862] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778408879.676500094] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 27860] (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408879.993677001] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408879.993751903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408881.922245432] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.475839 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408883.126215134] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.445481 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [1778408879.872023528] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] [INFO] [1778408885.801445590] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] 2026-05-10 12:28:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Running on http://127.0.0.1:54107 (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] * Running on http://172.30.43.138:54107 (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:28:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:28:05] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) + + +=================================== FAILURES =================================== +___________________________ test_suck_effector[ur5e] ___________________________ + +start_processes = Urls(ros_domain_id='85', robot_url='http://0.0.0.0:54107', storage_url='http://127.0.0.1:57693') + + @pytest.mark.timeout(60) + def test_suck_effector(start_processes: Urls) -> None: +  + robot_description = load_robot_description() + assert "suction_base_link" in robot_description + assert "suction_cup_link" in robot_description + assert "suction_tcp" in robot_description +> assert "tool0_to_suction_base" in robot_description +E assert 'tool0_to_suction_base' in '\n\n\n\n\n\n Read time : 12.313 us, Update time : 9432.433 us (Switch time : 9384.542 us (Switch chained mode time : 0.662 us, perform mode change time : 10.710 us, Activation time : 9358.082 us, Deactivation time : 0.160 us)), Write time : 3.337 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408878.374356510] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.586517 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.374307067] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.376164455] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.377239573] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.377271183] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.377704972] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.390096869] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.405246826] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.406319725] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.406390429] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.410969053] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.412893871] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.417552378] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.419227010] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.419244694] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.421780647] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.450187558] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.453977926] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.462150650] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.464234079] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.465000675] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.468158520] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.470198538] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.470217594] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.470656363] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.488204049] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.488568136] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.491711058] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.492998349] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.495978237] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.497369735] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.497405874] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.497744688] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.510989105] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.528010061] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528574694] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528749977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528765727] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528775596] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.531864716] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.554122152] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.555398052] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.570292217] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 27862] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408879.676500094] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 27860] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408879.993677001] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408879.993751903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408881.922245432] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.475839 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408883.126215134] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.445481 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408879.872023528] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408885.801445590] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:28:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:54107 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:54107 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:28:05] "GET /healthz/ready HTTP/1.1" 200 - +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_suck_effector.py:10 + src/python/arcor2_ur/tests/test_suck_effector.py:10: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(60) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_suck_effector.py.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_suck_effector.py::test_suck_effector[ur5e] - assert 'tool0_to_suction_base' in '\n Read time : 14.507 us, Update time : 9700.844 us (Switch time : 9654.205 us (Switch chained mode time : 0.381 us, perform mode change time : 12.224 us, Activation time : 9624.458 us, Deactivation time : 0.180 us)), Write time : 2.254 us (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408966.620408695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.880177 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.620377296] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.621643902] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.622858525] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.622899924] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.623466775] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.642939124] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.645841138] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.646239791] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.646337587] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.649637192] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.650771587] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.653654304] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.655265967] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.655304480] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.656640855] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.677946071] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.678323414] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.680797175] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.683626491] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.684871446] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.687779657] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.689074823] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.689114148] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.689516392] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.707979148] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.708350514] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.711451221] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.712762358] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.715883604] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.717353260] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.717399999] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.717955419] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.735359690] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.752307543] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.752981192] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.753122721] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.753137639] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.753147398] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.754931753] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.764345126] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.764965224] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.768961181] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [robot_state_publisher-2] [INFO] [1778408966.831056325] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 30307] (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 30305] (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408967.952895816] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408967.952950620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408969.067815810] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.287954 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408970.242220186] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.692100 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408973.093370250] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.842163 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408974.097789566] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.261560 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408975.182221851] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.693895 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408975.493127231] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408975.493178468] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408975.761371324] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408975.761406781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.525708663] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.180627 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.759130170] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.759166860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.759109321] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408977.759157713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [INFO] [1778408967.838581671] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] [INFO] [1778408973.421448284] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:29:38 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] * Running on http://127.0.0.1:43109 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] * Running on http://172.30.43.138:43109 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.759627551] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.759664791] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.759674400] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.767103696] [moveit_1321688465.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.769480512] [moveit_1321688465.moveit.ros.rdf_loader]: Loaded robot model in 0.00218947 seconds (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.769521229] [moveit_1321688465.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.769534505] [moveit_1321688465.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [WARN] [1778408976.782308422] [moveit_1321688465.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.784827117] [moveit_1321688465.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.842885861] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.843029724] [moveit_1321688465.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844159581] [moveit_1321688465.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844705848] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844721188] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844836857] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845239081] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845329123] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845741656] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845753619] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.846418929] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.846909572] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [WARN] [1778408976.847130171] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [ERROR] [1778408976.847146892] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.026190048] [moveit_1321688465.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.027012000] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.028560699] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.028580166] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029042294] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029060158] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029087530] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029092890] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029104763] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.030251060] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.032489603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.032505664] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.034504872] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.034520872] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.035246021] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.064593754] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.068137964] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.068329783] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.068354781] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.069257156] [moveit_1321688465.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:37] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:38] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408978.253042507] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:53 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:53 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:53 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:29:57 [ ERROR] [INFO] [1778408993.396324103] [moveit_1321688465.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +______________________ test_pick_up_box_by_position[ur5e] ______________________ + +start_processes = Urls(ros_domain_id='55', robot_url='http://0.0.0.0:43109', storage_url='http://127.0.0.1:40591') + + @pytest.mark.timeout(321) + def test_pick_up_box_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Box("Box1", 0.2, 0.2, 0.2) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Box1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-24-826127-DESKTOP-HG3Q5KV-30285 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [30304] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [30305] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [30306] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [30307] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [30308] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408965.114460682] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.118941956] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.121471017] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.124139793] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.124167926] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.124173507] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408965.124265422] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.291228511] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.293916068] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.294385070] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.294478658] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.302275922] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303637694] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303673372] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303698179] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303704060] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303766679] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303807537] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.555397724] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.807211325] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.807264436] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.809804657] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.831778494] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.832828425] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.832966127] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.833002666] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.833025549] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.837011313] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.843725250] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.843767550] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.844659315] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.855331924] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.855559702] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.856318263] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.857725581] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.857759746] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.858483271] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.869374897] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.869601437] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.870226406] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.871847991] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.871883598] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.872555701] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.883458458] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.883674317] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.884210616] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.885827504] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.885861669] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.889386251] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.901494819] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.901737665] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.910012879] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.910050861] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.910369797] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.923699049] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.923939596] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.929747397] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.929778646] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.930051073] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.943489835] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.943706270] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.945973829] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.946027751] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.946604437] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.959742242] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.960028134] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.056723572] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 30308] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.564776222] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.564842337] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.566640233] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.583650610] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.584930797] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.585002874] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.590293423] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.590828789] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.593816360] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.594727577] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.594762874] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.595055420] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.607495067] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.607790885] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.609344350] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408966.620328632] [controller_manager]: Overrun might occur, Total time : 9717.605 us (Expected < 2000.000 us) --> Read time : 14.507 us, Update time : 9700.844 us (Switch time : 9654.205 us (Switch chained mode time : 0.381 us, perform mode change time : 12.224 us, Activation time : 9624.458 us, Deactivation time : 0.180 us)), Write time : 2.254 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408966.620408695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.880177 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.620377296] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.621643902] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.622858525] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.622899924] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.623466775] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.642939124] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.645841138] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.646239791] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.646337587] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.649637192] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.650771587] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.653654304] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.655265967] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.655304480] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.656640855] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.677946071] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.678323414] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.680797175] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.683626491] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.684871446] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.687779657] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.689074823] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.689114148] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.689516392] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.707979148] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.708350514] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.711451221] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.712762358] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.715883604] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.717353260] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.717399999] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.717955419] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.735359690] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.752307543] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.752981192] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.753122721] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.753137639] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.753147398] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.754931753] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.764345126] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.764965224] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.768961181] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408966.831056325] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 30307] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 30305] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408967.952895816] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408967.952950620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408969.067815810] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.287954 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408970.242220186] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.692100 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408973.093370250] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.842163 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408974.097789566] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.261560 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408975.182221851] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.693895 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408975.493127231] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408975.493178468] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408975.761371324] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408975.761406781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.525708663] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.180627 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.759130170] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.759166860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.759109321] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408977.759157713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408967.838581671] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408973.421448284] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:43109 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:43109 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.759627551] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.759664791] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.759674400] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.767103696] [moveit_1321688465.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.769480512] [moveit_1321688465.moveit.ros.rdf_loader]: Loaded robot model in 0.00218947 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.769521229] [moveit_1321688465.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.769534505] [moveit_1321688465.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408976.782308422] [moveit_1321688465.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.784827117] [moveit_1321688465.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.842885861] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.843029724] [moveit_1321688465.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844159581] [moveit_1321688465.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844705848] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844721188] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844836857] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845239081] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845329123] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845741656] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845753619] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.846418929] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.846909572] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408976.847130171] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778408976.847146892] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.026190048] [moveit_1321688465.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.027012000] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.028560699] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.028580166] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029042294] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029060158] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029087530] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029092890] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029104763] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.030251060] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.032489603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.032505664] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.034504872] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.034520872] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.035246021] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.064593754] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.068137964] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.068329783] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.068354781] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.069257156] [moveit_1321688465.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:37] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:38] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408978.253042507] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:53 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:53 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:53 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408993.396324103] [moveit_1321688465.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:13 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_box_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py::test_pick_up_box_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 32.96s ========================= + + +12:30:09.05 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests +12:30:09.05 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-M0LXPH +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py::test_pick_up_sphere_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:29:49 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-36-208332-DESKTOP-HG3Q5KV-31089 (testutils.py:33) + +2026-05-10 12:29:49 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [31132] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [31133] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [31134] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-4]: process started with pid [31135] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-5]: process started with pid [31136] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778408976.532131849] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.538816220] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.541378107] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.543599518] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.543626990] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.543632180] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.543711736] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.712179565] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.715022676] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.715407007] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.715569145] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.723447589] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725043958] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725066681] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725097730] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725106938] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725180808] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725238688] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408976.847930090] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.870043417] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.927368 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.100371637] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.100414398] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.103425614] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.123631408] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125002904] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125280957] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125347443] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125374014] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.130716450] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.137757945] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.137803892] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.138929151] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.151276111] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.151567825] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.152423361] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.155483089] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.155519980] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.156208939] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.169217080] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.169756591] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.170325041] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.173431658] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.173467446] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.174155303] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.187297154] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.187556834] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.188103362] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.191662845] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.191708882] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.195586185] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.209227480] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.209505503] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.217730906] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.217775701] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.218087849] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.233220754] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.233461055] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.239684792] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.239724597] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.240073675] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.253174870] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.253446681] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.255739793] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.255776443] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.256341391] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.271272187] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.271598111] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.396783636] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 31136] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.649546875] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.649609775] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.651136791] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.667450453] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.668743921] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.668804295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.673545174] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.674382585] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.677424104] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.678354858] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.678399773] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.678707297] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.691242857] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.691527402] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.692892381] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408977.704017285] [controller_manager]: Overrun might occur, Total time : 9843.524 us (Expected < 2000.000 us) --> Read time : 16.481 us, Update time : 9823.777 us (Switch time : 9768.661 us (Switch chained mode time : 0.431 us, perform mode change time : 14.798 us, Activation time : 9736.400 us, Deactivation time : 0.251 us)), Write time : 3.266 us (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.704036285] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.705518336] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.706683084] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.706724152] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.707090093] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.719565945] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.721550179] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.721975667] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.722040971] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.725085075] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.726296251] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.729448982] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.730892896] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.730927481] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.732069777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.751676412] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.751990223] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.754098475] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.755524634] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.756300214] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.759461649] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.760563018] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.760604086] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.761041777] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.777268744] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.777598496] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.781163922] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.782344650] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.785230733] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.786439094] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.786476435] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.786855476] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.800355021] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.813413029] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.813794809] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.813950234] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.813988898] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.814000470] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.815540661] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.823333378] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.824469884] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.827313816] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 31135] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778408978.986796856] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31133] (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408979.256160972] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408979.256213061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408979.730218754] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.103215 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408980.410161770] [controller_manager]: Overrun might occur, Total time : 4012.994 us (Expected < 2000.000 us) --> Read time : 13.275 us, Update time : 3997.996 us, Write time : 1.723 us (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408981.194205176] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.089216 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408982.324902042] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.786393 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408985.170836516] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.720827 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408985.652126918] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408985.652184448] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408985.900973482] [controller_manager]: Overrun might occur, Total time : 2445.335 us (Expected < 2000.000 us) --> Read time : 14.387 us, Update time : 2429.245 us, Write time : 1.703 us (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408986.652330799] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408986.652378199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408987.652348365] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408987.652411194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408988.652280638] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408988.652332526] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [1778408979.144732481] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] [INFO] [1778408989.644691271] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:29:49 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] * Running on http://127.0.0.1:45973 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] * Running on http://172.30.43.138:45973 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:44] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:45] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:45] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:45] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.030948253] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.031000402] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.031017494] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.038319258] [moveit_3024616344.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.040199040] [moveit_3024616344.moveit.ros.rdf_loader]: Loaded robot model in 0.0017393 seconds (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.040227244] [moveit_3024616344.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.040234498] [moveit_3024616344.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [WARN] [1778408988.051601396] [moveit_3024616344.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.054261235] [moveit_3024616344.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.105560063] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.105680061] [moveit_3024616344.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.106788242] [moveit_3024616344.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107389785] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107406727] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107530412] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107941714] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.108032707] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.108699539] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.108713586] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.109143903] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.109605441] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [WARN] [1778408988.109888098] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [ERROR] [1778408988.109907495] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.327276701] [moveit_3024616344.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.327992271] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329105215] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329118861] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329488173] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329503632] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329535042] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329545071] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329560129] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.330436805] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.332819794] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.332835243] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.335250452] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.335268376] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.336139982] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.369525108] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.373596495] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.373795773] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.373829197] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.374643979] [moveit_3024616344.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:49] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:49] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778408989.775074879] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:30:04 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:30:04 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:30:04 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:30:08 [ ERROR] [INFO] [1778409004.931996196] [moveit_3024616344.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +____________________ test_pick_up_sphere_by_position[ur5e] _____________________ + +start_processes = Urls(ros_domain_id='152', robot_url='http://0.0.0.0:45973', storage_url='http://127.0.0.1:44981') + + @pytest.mark.timeout(321) + def test_pick_up_sphere_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Sphere("Sphere1", 0.1) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Sphere1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-36-208332-DESKTOP-HG3Q5KV-31089 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [31132] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [31133] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [31134] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [31135] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [31136] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408976.532131849] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.538816220] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.541378107] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.543599518] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.543626990] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.543632180] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.543711736] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.712179565] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.715022676] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.715407007] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.715569145] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.723447589] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725043958] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725066681] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725097730] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725106938] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725180808] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725238688] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408976.847930090] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.870043417] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.927368 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.100371637] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.100414398] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.103425614] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.123631408] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125002904] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125280957] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125347443] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125374014] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.130716450] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.137757945] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.137803892] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.138929151] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.151276111] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.151567825] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.152423361] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.155483089] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.155519980] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.156208939] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.169217080] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.169756591] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.170325041] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.173431658] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.173467446] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.174155303] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.187297154] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.187556834] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.188103362] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.191662845] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.191708882] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.195586185] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.209227480] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.209505503] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.217730906] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.217775701] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.218087849] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.233220754] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.233461055] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.239684792] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.239724597] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.240073675] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.253174870] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.253446681] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.255739793] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.255776443] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.256341391] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.271272187] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.271598111] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.396783636] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 31136] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.649546875] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.649609775] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.651136791] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.667450453] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.668743921] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.668804295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.673545174] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.674382585] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.677424104] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.678354858] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.678399773] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.678707297] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.691242857] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.691527402] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.692892381] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408977.704017285] [controller_manager]: Overrun might occur, Total time : 9843.524 us (Expected < 2000.000 us) --> Read time : 16.481 us, Update time : 9823.777 us (Switch time : 9768.661 us (Switch chained mode time : 0.431 us, perform mode change time : 14.798 us, Activation time : 9736.400 us, Deactivation time : 0.251 us)), Write time : 3.266 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.704036285] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.705518336] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.706683084] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.706724152] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.707090093] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.719565945] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.721550179] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.721975667] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.722040971] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.725085075] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.726296251] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.729448982] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.730892896] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.730927481] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.732069777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.751676412] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.751990223] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.754098475] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.755524634] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.756300214] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.759461649] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.760563018] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.760604086] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.761041777] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.777268744] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.777598496] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.781163922] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.782344650] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.785230733] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.786439094] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.786476435] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.786855476] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.800355021] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.813413029] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.813794809] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.813950234] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.813988898] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.814000470] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.815540661] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.823333378] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.824469884] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.827313816] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 31135] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408978.986796856] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31133] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408979.256160972] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408979.256213061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408979.730218754] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.103215 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408980.410161770] [controller_manager]: Overrun might occur, Total time : 4012.994 us (Expected < 2000.000 us) --> Read time : 13.275 us, Update time : 3997.996 us, Write time : 1.723 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408981.194205176] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.089216 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408982.324902042] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.786393 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408985.170836516] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.720827 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408985.652126918] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408985.652184448] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408985.900973482] [controller_manager]: Overrun might occur, Total time : 2445.335 us (Expected < 2000.000 us) --> Read time : 14.387 us, Update time : 2429.245 us, Write time : 1.703 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408986.652330799] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408986.652378199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408987.652348365] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408987.652411194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408988.652280638] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408988.652332526] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408979.144732481] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408989.644691271] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:45973 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:45973 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:44] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:45] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:45] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:45] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.030948253] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.031000402] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.031017494] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.038319258] [moveit_3024616344.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.040199040] [moveit_3024616344.moveit.ros.rdf_loader]: Loaded robot model in 0.0017393 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.040227244] [moveit_3024616344.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.040234498] [moveit_3024616344.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408988.051601396] [moveit_3024616344.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.054261235] [moveit_3024616344.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.105560063] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.105680061] [moveit_3024616344.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.106788242] [moveit_3024616344.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107389785] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107406727] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107530412] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107941714] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.108032707] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.108699539] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.108713586] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.109143903] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.109605441] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408988.109888098] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778408988.109907495] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.327276701] [moveit_3024616344.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.327992271] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329105215] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329118861] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329488173] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329503632] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329535042] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329545071] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329560129] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.330436805] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.332819794] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.332835243] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.335250452] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.335268376] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.336139982] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.369525108] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.373596495] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.373795773] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.373829197] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.374643979] [moveit_3024616344.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:49] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:49] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408989.775074879] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409004.931996196] [moveit_3024616344.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:13 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_sphere_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py::test_pick_up_sphere_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 33.05s ========================= + + +12:30:34.98 [INFO] Long running tasks: + 65.52s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests +12:30:39.57 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests +12:30:39.58 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-EPF00Y +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py::test_overhead_box[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:30:21 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-56-696264-DESKTOP-HG3Q5KV-31785 (testutils.py:33) + +2026-05-10 12:30:21 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [31792] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [31793] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [31794] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-4]: process started with pid [31795] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-5]: process started with pid [31796] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778408997.075826168] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.091213456] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.094959849] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.098099809] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.098131529] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.098374862] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408997.098521325] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.262584014] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.265282091] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.265616988] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.265666091] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.274549632] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276076593] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276115477] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276141877] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276148008] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276222410] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276282724] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.368266884] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.620767270] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.620818156] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.624132694] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408997.630046397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.267420 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.644042796] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645269107] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645444440] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645480598] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645502330] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.650400321] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.658278886] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.658322038] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.659219563] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.671871063] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.672163334] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.672939964] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.675891227] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.675925212] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.676774911] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.689813387] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.690126871] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.690625950] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.691948232] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.691984872] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.692584051] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.704144933] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.704432229] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.704991502] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.708492905] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.708525026] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.712287966] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.725961027] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.726241034] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.734424809] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.734466408] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.734794271] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.747960768] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.748244317] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.754492033] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.754529464] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.754827961] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.768213194] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.768487131] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.772488984] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.772526055] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.773097390] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.787756119] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.788031146] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408997.931075754] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 31796] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.184148316] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.184245441] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.185784900] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.202030494] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.203319769] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.203383029] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.208649421] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.209091591] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.212127350] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.213036537] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.213067636] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.213405268] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.227737962] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.228018956] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.229516351] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408998.240572529] [controller_manager]: Overrun might occur, Total time : 9714.758 us (Expected < 2000.000 us) --> Read time : 13.525 us, Update time : 9699.139 us (Switch time : 9654.174 us (Switch chained mode time : 0.401 us, perform mode change time : 10.681 us, Activation time : 9628.846 us, Deactivation time : 0.170 us)), Write time : 2.094 us (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.240593855] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.241862610] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.242895367] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.242934972] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.243380749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.256437498] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.257842874] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.258110317] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.258166133] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.259704411] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.260967801] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.264109406] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.265285346] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.265320413] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.266422096] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.283771757] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.284039070] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.286017098] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.287665035] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.288977478] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.291895062] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.292986010] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.293027339] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.293320586] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.308005730] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.308380031] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.311745756] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.313007394] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.316009398] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.317300651] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.317331079] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.317638423] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.332088124] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.346500666] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347027667] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347199564] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347259548] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347276029] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.348974706] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.355649955] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.357159448] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.360175038] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 31795] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778408999.456590096] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31793] (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.753389224] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408999.753434510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.685824435] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.045749 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.734163094] [controller_manager]: Overrun might occur, Total time : 3322.173 us (Expected < 2000.000 us) --> Read time : 15.990 us, Update time : 3304.570 us, Write time : 1.613 us (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409002.816354397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.575370 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409004.955654364] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.875678 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409005.848364594] [controller_manager]: Overrun might occur, Total time : 3533.003 us (Expected < 2000.000 us) --> Read time : 15.079 us, Update time : 3516.101 us, Write time : 1.823 us (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409006.302054930] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.302105346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.714830477] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.051360 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409007.302447423] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409007.302524730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409007.825926331] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.147565 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409008.302560631] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409008.302626716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.269975539] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.196803 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409009.305686925] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.305720328] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.302340950] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.302403238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.403583042] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.954689770] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.302293714] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.302333429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.378954189] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.175212 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.522961023] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.523028521] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409012.302401106] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.302455079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.169982343] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.203317 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.302385581] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.302439824] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.397415855] [controller_manager]: Overrun might occur, Total time : 2555.311 us (Expected < 2000.000 us) --> Read time : 196.584 us, Update time : 2355.250 us, Write time : 3.477 us (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.302399895] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.302457845] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.550287817] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.508870 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409015.302308050] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409015.302361051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409016.302368400] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.302422954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.546304253] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.525577 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.697812521] [controller_manager]: Overrun might occur, Total time : 2473.369 us (Expected < 2000.000 us) --> Read time : 19.567 us, Update time : 2451.929 us, Write time : 1.873 us (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409017.302685362] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.302746348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.681561355] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.782258 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409018.302889468] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409018.302947558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.147569303] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.790026 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409019.302348651] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.302404777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.439028730] [controller_manager]: Overrun might occur, Total time : 5949.926 us (Expected < 2000.000 us) --> Read time : 441.348 us, Update time : 5504.240 us, Write time : 4.338 us (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409020.306165682] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.306223072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.344654875] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.876119 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409021.302523291] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.302568958] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.380114272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.335556 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [1778408999.633011969] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] [INFO] [1778409001.558812388] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:30:21 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:04 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] * Running on http://127.0.0.1:49437 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] * Running on http://172.30.43.138:49437 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.939037878] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.939095227] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.939106769] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.945591020] [moveit_1524511032.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.947918924] [moveit_1524511032.moveit.ros.rdf_loader]: Loaded robot model in 0.00216334 seconds (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.947957707] [moveit_1524511032.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.947968157] [moveit_1524511032.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [WARN] [1778409008.960680858] [moveit_1524511032.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.963945891] [moveit_1524511032.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.022826706] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.022995417] [moveit_1524511032.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.023988775] [moveit_1524511032.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.024694626] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.024712009] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.024835313] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.025359129] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.025448890] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.026187768] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.026205782] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.026678390] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.027105001] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [WARN] [1778409009.027304901] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [ERROR] [1778409009.027315751] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.071683044] [moveit_1524511032.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.072617791] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074270126] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074288942] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074693811] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074711685] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074737855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074742363] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074751851] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.076117260] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.079019074] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.079033892] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.084554161] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.084576053] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.085796331] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.447130590] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.450460498] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.450637975] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.450667070] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.451601651] [moveit_1524511032.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:10] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:10] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:11 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632727433] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632852721] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632862559] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632880694] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [WARN] [1778409011.632980183] [moveit_1524511032.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.633454546] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.633558343] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [ERROR] [1778409021.634133598] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [ERROR] [1778409021.634272271] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:21 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:21] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409021.876359631] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:36 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:36 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:36 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:30:39 [ ERROR] [INFO] [1778409037.016289503] [moveit_1524511032.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +___________________________ test_overhead_box[ur5e] ____________________________ + +start_processes = Urls(ros_domain_id='217', robot_url='http://0.0.0.0:49437', storage_url='http://127.0.0.1:34855') + + @pytest.mark.timeout(400) + def test_overhead_box(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +> ot.move_to_pose("", start_pose, 0.3, safe=False) + +src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-56-696264-DESKTOP-HG3Q5KV-31785 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [31792] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [31793] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [31794] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [31795] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [31796] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408997.075826168] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.091213456] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.094959849] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.098099809] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.098131529] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.098374862] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408997.098521325] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.262584014] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.265282091] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.265616988] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.265666091] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.274549632] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276076593] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276115477] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276141877] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276148008] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276222410] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276282724] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.368266884] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.620767270] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.620818156] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.624132694] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408997.630046397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.267420 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.644042796] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645269107] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645444440] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645480598] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645502330] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.650400321] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.658278886] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.658322038] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.659219563] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.671871063] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.672163334] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.672939964] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.675891227] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.675925212] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.676774911] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.689813387] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.690126871] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.690625950] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.691948232] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.691984872] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.692584051] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.704144933] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.704432229] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.704991502] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.708492905] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.708525026] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.712287966] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.725961027] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.726241034] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.734424809] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.734466408] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.734794271] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.747960768] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.748244317] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.754492033] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.754529464] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.754827961] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.768213194] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.768487131] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.772488984] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.772526055] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.773097390] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.787756119] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.788031146] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408997.931075754] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 31796] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.184148316] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.184245441] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.185784900] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.202030494] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.203319769] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.203383029] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.208649421] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.209091591] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.212127350] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.213036537] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.213067636] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.213405268] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.227737962] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.228018956] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.229516351] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408998.240572529] [controller_manager]: Overrun might occur, Total time : 9714.758 us (Expected < 2000.000 us) --> Read time : 13.525 us, Update time : 9699.139 us (Switch time : 9654.174 us (Switch chained mode time : 0.401 us, perform mode change time : 10.681 us, Activation time : 9628.846 us, Deactivation time : 0.170 us)), Write time : 2.094 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.240593855] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.241862610] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.242895367] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.242934972] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.243380749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.256437498] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.257842874] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.258110317] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.258166133] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.259704411] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.260967801] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.264109406] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.265285346] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.265320413] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.266422096] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.283771757] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.284039070] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.286017098] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.287665035] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.288977478] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.291895062] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.292986010] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.293027339] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.293320586] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.308005730] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.308380031] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.311745756] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.313007394] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.316009398] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.317300651] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.317331079] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.317638423] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.332088124] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.346500666] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347027667] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347199564] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347259548] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347276029] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.348974706] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.355649955] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.357159448] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.360175038] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 31795] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408999.456590096] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31793] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.753389224] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408999.753434510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.685824435] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.045749 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.734163094] [controller_manager]: Overrun might occur, Total time : 3322.173 us (Expected < 2000.000 us) --> Read time : 15.990 us, Update time : 3304.570 us, Write time : 1.613 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409002.816354397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.575370 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409004.955654364] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.875678 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409005.848364594] [controller_manager]: Overrun might occur, Total time : 3533.003 us (Expected < 2000.000 us) --> Read time : 15.079 us, Update time : 3516.101 us, Write time : 1.823 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409006.302054930] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.302105346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.714830477] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.051360 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409007.302447423] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409007.302524730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409007.825926331] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.147565 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409008.302560631] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409008.302626716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.269975539] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.196803 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409009.305686925] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.305720328] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.302340950] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.302403238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.403583042] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.954689770] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.302293714] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.302333429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.378954189] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.175212 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.522961023] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.523028521] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409012.302401106] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.302455079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.169982343] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.203317 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.302385581] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.302439824] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.397415855] [controller_manager]: Overrun might occur, Total time : 2555.311 us (Expected < 2000.000 us) --> Read time : 196.584 us, Update time : 2355.250 us, Write time : 3.477 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.302399895] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.302457845] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.550287817] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.508870 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409015.302308050] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409015.302361051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409016.302368400] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.302422954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.546304253] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.525577 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.697812521] [controller_manager]: Overrun might occur, Total time : 2473.369 us (Expected < 2000.000 us) --> Read time : 19.567 us, Update time : 2451.929 us, Write time : 1.873 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409017.302685362] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.302746348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.681561355] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.782258 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409018.302889468] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409018.302947558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.147569303] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.790026 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409019.302348651] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.302404777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.439028730] [controller_manager]: Overrun might occur, Total time : 5949.926 us (Expected < 2000.000 us) --> Read time : 441.348 us, Update time : 5504.240 us, Write time : 4.338 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409020.306165682] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.306223072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.344654875] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.876119 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409021.302523291] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.302568958] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.380114272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.335556 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408999.633011969] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409001.558812388] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:49437 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:49437 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.939037878] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.939095227] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.939106769] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.945591020] [moveit_1524511032.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.947918924] [moveit_1524511032.moveit.ros.rdf_loader]: Loaded robot model in 0.00216334 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.947957707] [moveit_1524511032.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.947968157] [moveit_1524511032.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409008.960680858] [moveit_1524511032.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.963945891] [moveit_1524511032.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.022826706] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.022995417] [moveit_1524511032.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.023988775] [moveit_1524511032.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.024694626] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.024712009] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.024835313] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.025359129] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.025448890] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.026187768] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.026205782] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.026678390] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.027105001] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409009.027304901] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409009.027315751] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.071683044] [moveit_1524511032.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.072617791] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074270126] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074288942] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074693811] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074711685] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074737855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074742363] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074751851] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.076117260] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.079019074] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.079033892] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.084554161] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.084576053] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.085796331] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.447130590] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.450460498] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.450637975] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.450667070] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.451601651] [moveit_1524511032.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:10] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:10] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632727433] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632852721] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632862559] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632880694] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409011.632980183] [moveit_1524511032.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.633454546] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.633558343] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409021.634133598] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409021.634272271] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:21] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409021.876359631] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409037.016289503] [moveit_1524511032.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:12 + src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_overhead_obstacle_box.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py::test_overhead_box[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed +======================== 1 failed, 1 warning in 43.16s ========================= + + +12:30:49.05 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests +12:30:49.05 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-iQVxGl +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py::test_side_cylinder[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:30:31 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-58-744636-DESKTOP-HG3Q5KV-31946 (testutils.py:33) + +2026-05-10 12:30:31 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [31953] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [31954] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [31955] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-4]: process started with pid [31956] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-5]: process started with pid [31957] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [robot_state_publisher-2] [INFO] [1778408999.018787519] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.026245059] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.029076339] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.032108079] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.032148777] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.032159577] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778408999.032281970] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.196684494] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.199575428] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.199984425] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.200050791] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.208610119] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209898112] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209938097] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209967774] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209973214] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.210037316] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.210087571] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.301775740] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [robot_state_publisher-2] [INFO] [1778408999.456604157] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.553738109] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.553806088] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.556827178] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31954] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.573988176] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575031854] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575199993] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575231503] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575254717] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.579660600] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.585689029] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.585726080] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.586587026] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.597469934] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.597743735] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.598429628] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.599612370] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.599652957] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.600197502] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.611847644] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.612132265] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.612712818] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.615895645] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.615928488] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.616454858] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.627547190] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.627791981] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.628290217] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.629685027] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.629717138] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.633376186] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.643816469] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.644074038] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.652029134] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.652067867] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.652345685] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.663591523] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.663867517] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.668057479] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.668092466] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.668424046] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.679879923] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.680177789] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.684348424] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.684383240] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.684907696] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.700323563] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.700755238] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778408999.808631471] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 31957] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.061036841] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.061084651] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.062516612] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.075693365] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.076606064] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.076664144] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.082076244] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.082822902] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.085608175] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.086504392] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.086540050] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.086900234] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.099527203] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.099845066] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.101427227] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409000.110469304] [controller_manager]: Overrun might occur, Total time : 7871.879 us (Expected < 2000.000 us) --> Read time : 11.733 us, Update time : 7857.922 us (Switch time : 7817.054 us (Switch chained mode time : 0.341 us, perform mode change time : 10.500 us, Activation time : 7790.223 us, Deactivation time : 0.140 us)), Write time : 2.224 us (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409000.110536271] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.015016 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.110480034] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.113878732] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.114791201] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.114820857] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.115244147] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.126690731] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.129627393] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.129925324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.129982412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.133343131] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.134706953] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.137539772] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.138636650] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.138673190] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.139745577] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.155429869] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.155692594] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.157674609] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.159416796] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.160773183] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.163671068] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.164707834] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.164745896] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.164981123] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.177589622] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.177909223] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.181346028] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.182723124] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.185585753] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.186685964] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.186737602] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.186996444] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.199418583] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.211744318] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212081008] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212252384] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212290206] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212301057] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.213704162] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.219737311] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.220911061] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.223906728] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 31956] (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.685739664] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.218930 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409001.902787781] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.902865669] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409002.818200709] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.680145 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409004.955824146] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.303502 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.712170964] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.650100 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.768863972] [controller_manager]: Overrun might occur, Total time : 5764.203 us (Expected < 2000.000 us) --> Read time : 28.094 us, Update time : 5734.216 us, Write time : 1.893 us (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409007.825452135] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930940 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409008.481078451] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409008.481134999] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.269893342] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.372468 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409009.481409649] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.481466267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.481271029] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.481320062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.481317806] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.481366458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.879705361] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.184547 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.205388461] [controller_manager]: Overrun might occur, Total time : 2496.594 us (Expected < 2000.000 us) --> Read time : 44.880 us, Update time : 2447.286 us, Write time : 4.428 us (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409012.247123489] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409012.481498403] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.481545182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.798347172] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.073235669] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.714985 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.354328407] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.354388000] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.393956591] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.394024249] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.398033098] [controller_manager]: Overrun might occur, Total time : 3429.098 us (Expected < 2000.000 us) --> Read time : 23.094 us, Update time : 3402.477 us, Write time : 3.527 us (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.481509953] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.481567272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.481203599] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.481258443] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409015.342309325] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.788460 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409015.481320373] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409015.481687260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409016.016631026] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409016.481494817] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.481538019] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.545472938] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.952314 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409017.481322328] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.481370599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409018.481496856] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409018.481550728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.420874128] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.353224 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.437336565] [controller_manager]: Overrun might occur, Total time : 3371.067 us (Expected < 2000.000 us) --> Read time : 41.910 us, Update time : 3323.867 us, Write time : 5.290 us (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409019.481814993] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.481895636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409020.481716499] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.481756555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.502572992] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.051858 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.626193085] [controller_manager]: Overrun might occur, Total time : 3608.958 us (Expected < 2000.000 us) --> Read time : 19.236 us, Update time : 3587.227 us, Write time : 2.495 us (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409021.481374444] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.481416384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409022.481276873] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409022.481328210] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409023.481120312] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409023.481172571] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409024.481344627] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409024.481399341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409025.481388742] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409025.481445921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409026.481290626] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409026.481346903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409027.481342380] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409027.481401342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409028.481265890] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409028.481325343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409029.481288400] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409029.481342152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409030.481355747] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409030.481431941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [1778409001.780580619] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] [INFO] [1778409013.066125273] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:30:31 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] * Running on http://127.0.0.1:51469 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] * Running on http://172.30.43.138:51469 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.717336898] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.717395339] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.717409035] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.721851099] [moveit_3334433294.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.724119631] [moveit_3334433294.moveit.ros.rdf_loader]: Loaded robot model in 0.00211411 seconds (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.724149488] [moveit_3334433294.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.724157253] [moveit_3334433294.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [WARN] [1778409010.737205550] [moveit_3334433294.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.740801880] [moveit_3334433294.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.801893169] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.802069139] [moveit_3334433294.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803152187] [moveit_3334433294.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803697854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803716148] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803879303] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804243665] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804340619] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804882854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804899907] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.805366068] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.805799711] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [WARN] [1778409010.806045368] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [ERROR] [1778409010.806063823] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.782611224] [moveit_3334433294.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.783615122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785044428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785058234] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785345721] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785357453] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785379745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785383853] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785393582] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.786151592] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.788536603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.788549828] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.790883823] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.790900395] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.791659397] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.813791671] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.816967616] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.817127911] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.817160843] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.817864781] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:12] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:12] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365168505] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365305395] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365325083] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365349299] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [WARN] [1778409013.365463405] [moveit_3334433294.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365933469] [moveit_3334433294.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.366059839] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.383303483] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.384495418] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.384854039] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Executing plan (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392239799] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392270547] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392303129] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392348034] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392377410] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392969516] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.393005053] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.393028718] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.393170878] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.395063093] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.395082019] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409016.045000726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409016.050990843] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:31 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self._wait_for_pose_reached(target_abs, tolerance=0.01) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] raise UrGeneral("Target pose was not reached.") (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Target pose was not reached. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:31] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409031.265057490] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:46 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:46 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:46 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:30:48 [ ERROR] [INFO] [1778409046.402290002] [moveit_3334433294.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +___________________________ test_side_cylinder[ur5e] ___________________________ + +start_processes = Urls(ros_domain_id='126', robot_url='http://0.0.0.0:51469', storage_url='http://127.0.0.1:45425') + + @pytest.mark.timeout(400) + def test_side_cylinder(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +> ot.move_to_pose("", start_pose, 0.3, safe=False) + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not reached. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-58-744636-DESKTOP-HG3Q5KV-31946 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [31953] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [31954] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [31955] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [31956] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [31957] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408999.018787519] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.026245059] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.029076339] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.032108079] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.032148777] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.032159577] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408999.032281970] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.196684494] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.199575428] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.199984425] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.200050791] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.208610119] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209898112] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209938097] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209967774] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209973214] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.210037316] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.210087571] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.301775740] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408999.456604157] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.553738109] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.553806088] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.556827178] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31954] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.573988176] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575031854] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575199993] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575231503] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575254717] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.579660600] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.585689029] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.585726080] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.586587026] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.597469934] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.597743735] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.598429628] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.599612370] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.599652957] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.600197502] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.611847644] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.612132265] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.612712818] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.615895645] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.615928488] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.616454858] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.627547190] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.627791981] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.628290217] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.629685027] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.629717138] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.633376186] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.643816469] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.644074038] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.652029134] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.652067867] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.652345685] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.663591523] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.663867517] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.668057479] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.668092466] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.668424046] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.679879923] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.680177789] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.684348424] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.684383240] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.684907696] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.700323563] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.700755238] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408999.808631471] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 31957] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.061036841] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.061084651] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.062516612] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.075693365] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.076606064] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.076664144] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.082076244] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.082822902] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.085608175] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.086504392] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.086540050] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.086900234] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.099527203] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.099845066] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.101427227] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409000.110469304] [controller_manager]: Overrun might occur, Total time : 7871.879 us (Expected < 2000.000 us) --> Read time : 11.733 us, Update time : 7857.922 us (Switch time : 7817.054 us (Switch chained mode time : 0.341 us, perform mode change time : 10.500 us, Activation time : 7790.223 us, Deactivation time : 0.140 us)), Write time : 2.224 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409000.110536271] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.015016 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.110480034] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.113878732] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.114791201] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.114820857] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.115244147] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.126690731] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.129627393] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.129925324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.129982412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.133343131] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.134706953] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.137539772] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.138636650] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.138673190] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.139745577] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.155429869] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.155692594] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.157674609] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.159416796] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.160773183] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.163671068] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.164707834] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.164745896] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.164981123] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.177589622] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.177909223] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.181346028] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.182723124] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.185585753] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.186685964] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.186737602] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.186996444] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.199418583] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.211744318] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212081008] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212252384] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212290206] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212301057] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.213704162] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.219737311] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.220911061] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.223906728] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 31956] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.685739664] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.218930 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409001.902787781] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.902865669] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409002.818200709] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.680145 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409004.955824146] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.303502 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.712170964] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.650100 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.768863972] [controller_manager]: Overrun might occur, Total time : 5764.203 us (Expected < 2000.000 us) --> Read time : 28.094 us, Update time : 5734.216 us, Write time : 1.893 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409007.825452135] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930940 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409008.481078451] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409008.481134999] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.269893342] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.372468 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409009.481409649] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.481466267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.481271029] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.481320062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.481317806] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.481366458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.879705361] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.184547 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.205388461] [controller_manager]: Overrun might occur, Total time : 2496.594 us (Expected < 2000.000 us) --> Read time : 44.880 us, Update time : 2447.286 us, Write time : 4.428 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409012.247123489] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409012.481498403] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.481545182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.798347172] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.073235669] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.714985 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.354328407] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.354388000] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.393956591] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.394024249] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.398033098] [controller_manager]: Overrun might occur, Total time : 3429.098 us (Expected < 2000.000 us) --> Read time : 23.094 us, Update time : 3402.477 us, Write time : 3.527 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.481509953] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.481567272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.481203599] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.481258443] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409015.342309325] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.788460 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409015.481320373] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409015.481687260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409016.016631026] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409016.481494817] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.481538019] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.545472938] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.952314 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409017.481322328] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.481370599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409018.481496856] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409018.481550728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.420874128] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.353224 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.437336565] [controller_manager]: Overrun might occur, Total time : 3371.067 us (Expected < 2000.000 us) --> Read time : 41.910 us, Update time : 3323.867 us, Write time : 5.290 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409019.481814993] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.481895636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409020.481716499] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.481756555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.502572992] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.051858 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.626193085] [controller_manager]: Overrun might occur, Total time : 3608.958 us (Expected < 2000.000 us) --> Read time : 19.236 us, Update time : 3587.227 us, Write time : 2.495 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409021.481374444] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.481416384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409022.481276873] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409022.481328210] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409023.481120312] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409023.481172571] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409024.481344627] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409024.481399341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409025.481388742] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409025.481445921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409026.481290626] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409026.481346903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409027.481342380] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409027.481401342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409028.481265890] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409028.481325343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409029.481288400] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409029.481342152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409030.481355747] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409030.481431941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409001.780580619] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.066125273] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:51469 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:51469 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.717336898] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.717395339] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.717409035] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.721851099] [moveit_3334433294.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.724119631] [moveit_3334433294.moveit.ros.rdf_loader]: Loaded robot model in 0.00211411 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.724149488] [moveit_3334433294.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.724157253] [moveit_3334433294.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409010.737205550] [moveit_3334433294.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.740801880] [moveit_3334433294.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.801893169] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.802069139] [moveit_3334433294.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803152187] [moveit_3334433294.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803697854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803716148] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803879303] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804243665] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804340619] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804882854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804899907] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.805366068] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.805799711] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409010.806045368] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409010.806063823] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.782611224] [moveit_3334433294.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.783615122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785044428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785058234] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785345721] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785357453] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785379745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785383853] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785393582] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.786151592] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.788536603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.788549828] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.790883823] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.790900395] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.791659397] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.813791671] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.816967616] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.817127911] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.817160843] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.817864781] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:12] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:12] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365168505] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365305395] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365325083] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365349299] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409013.365463405] [moveit_3334433294.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365933469] [moveit_3334433294.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.366059839] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.383303483] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.384495418] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.384854039] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392239799] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392270547] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392303129] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392348034] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392377410] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392969516] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.393005053] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.393028718] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.393170878] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.395063093] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.395082019] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409016.045000726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409016.050990843] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:31 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._wait_for_pose_reached(target_abs, tolerance=0.01) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Target pose was not reached.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Target pose was not reached. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:31] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409031.265057490] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:46 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:46 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:46 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409046.402290002] [moveit_3334433294.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:12 + src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_cylinder.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py::test_side_cylinder[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not ... +======================== 1 failed, 1 warning in 50.51s ========================= + + +12:30:52.85 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests +12:30:55.84 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests +12:30:55.85 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-EUAiHN +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py::test_overhead_cylinder[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:30:37 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-10-302933-DESKTOP-HG3Q5KV-32555 (testutils.py:33) + +2026-05-10 12:30:37 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [32586] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [32587] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [32588] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-4]: process started with pid [32589] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-5]: process started with pid [32590] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [robot_state_publisher-2] [INFO] [1778409010.686719380] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.696301908] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.700265705] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.702719828] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.702736941] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.702741389] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.702980684] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.860846613] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.864053567] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.864502750] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.864580218] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.873333134] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874756919] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874796634] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874828325] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874835869] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874915590] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874969693] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409010.998711197] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.251069928] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.251140081] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.254010582] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.266969910] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.268098811] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.268206015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.277044884] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.277510007] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.280816815] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.281804592] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.281845991] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.287064242] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.298641395] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.299003282] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.299893428] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.311253639] [controller_manager]: Overrun might occur, Total time : 9947.300 us (Expected < 2000.000 us) --> Read time : 12.914 us, Update time : 9932.152 us (Switch time : 9888.690 us (Switch chained mode time : 0.691 us, perform mode change time : 10.560 us, Activation time : 9862.320 us, Deactivation time : 0.161 us)), Write time : 2.234 us (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.311326467] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.104770 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.311267836] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.314854471] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.315962932] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.316025521] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.316364194] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.326678598] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.328770634] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.329087426] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.329165674] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.332007584] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.333449334] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.336709729] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.337830128] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.337870545] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.338996394] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.356977304] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.357344462] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.359973292] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.362518594] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.363474275] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.366577315] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.367953771] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.367985101] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.368252769] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.385557587] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.385984272] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.390264464] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.391449962] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.394649252] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.396071629] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.396112647] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.396503649] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.411317257] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.425152336] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425514362] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425725223] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425781730] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425809663] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.428261043] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.436395799] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.437580595] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.440882559] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 32589] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409011.788984488] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.208551170] [controller_manager]: Overrun might occur, Total time : 3249.815 us (Expected < 2000.000 us) --> Read time : 2968.210 us, Update time : 278.930 us, Write time : 2.675 us (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [robot_state_publisher-2] [INFO] [1778409013.066150608] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.071854915] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.633579 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 32587] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.424310066] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.424372264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.543200365] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.543282021] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.543451092] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.545886393] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.664946 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.564706333] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566258499] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566402332] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566437689] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566453890] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.568179028] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.574716419] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.574742418] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.576399747] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.592742355] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.593038142] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.593972316] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.596952846] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.596975629] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.597811373] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.614667065] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.615405543] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.616021543] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.618713037] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.618735600] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.619651916] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.634685001] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.635574436] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.636260619] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.639338713] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.639358380] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.639811862] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.653444801] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.654268006] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.667567853] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.667605865] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.667918980] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.683011728] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.684173345] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.691723277] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.691764375] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.692469544] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.708776554] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.710233772] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.713331654] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.713365809] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.713633584] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.729014625] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.729872685] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 32590] (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.546436975] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.215869 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.669720656] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.499179 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.054380047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.158400 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.348171367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.950251 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409020.937156000] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.937210133] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.379388443] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.167117 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409021.936779045] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.936830403] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409022.936727011] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409022.936786224] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409023.936929120] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409023.936983644] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409024.936780949] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409024.936831305] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409025.398858931] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409025.937238058] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409025.937301318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409025.949971093] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409026.518345970] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409026.518418778] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409026.936841666] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409026.936884748] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409027.936869374] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409027.936929287] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409028.936981644] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409028.937193731] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409029.936771943] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409029.936830324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409030.936850265] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409030.936925698] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409031.936824906] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409031.936883608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409032.936864678] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409032.936918370] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409033.936777707] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409033.936829736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409034.936901721] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409034.936955754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409035.936853648] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409035.936907590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409036.936811406] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409036.936864317] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [1778409013.315860179] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] [INFO] [1778409037.213346839] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:30:37 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:19 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] * Running on http://127.0.0.1:36751 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] * Running on http://172.30.43.138:36751 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.245905266] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.245968446] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.245982232] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.252387979] [moveit_2882792308.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.254749016] [moveit_2882792308.moveit.ros.rdf_loader]: Loaded robot model in 0.00209667 seconds (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.254777200] [moveit_2882792308.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.254784704] [moveit_2882792308.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [WARN] [1778409023.266574569] [moveit_2882792308.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.269300339] [moveit_2882792308.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.325565849] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.325708581] [moveit_2882792308.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.326718009] [moveit_2882792308.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327277251] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327291939] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327398286] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327900061] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327988309] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.328585322] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.328598828] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.329053752] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.329505531] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [WARN] [1778409023.329734806] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [ERROR] [1778409023.329752169] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.488595075] [moveit_2882792308.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.489266010] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490406918] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490419261] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490752514] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490766451] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490790015] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490795185] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490810935] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.491639349] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.493935141] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.493950330] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.496156492] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.496170008] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.496809443] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.526223248] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.530075442] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.530245956] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.530271745] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.531111836] [moveit_2882792308.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:25] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:25] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:26 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547103323] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547236236] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547248579] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547267606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [WARN] [1778409026.547376312] [moveit_2882792308.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547832829] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547954100] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [ERROR] [1778409036.548437353] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [ERROR] [1778409036.548540139] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:36 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:37] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409037.342777967] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:52 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:52 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:52 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:30:55 [ ERROR] [INFO] [1778409052.485310461] [moveit_2882792308.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_________________________ test_overhead_cylinder[ur5e] _________________________ + +start_processes = Urls(ros_domain_id='187', robot_url='http://0.0.0.0:36751', storage_url='http://127.0.0.1:54545') + + @pytest.mark.timeout(400) + def test_overhead_cylinder(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +> ot.move_to_pose("", start_pose, 0.3, safe=False) + +src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-10-302933-DESKTOP-HG3Q5KV-32555 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [32586] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [32587] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [32588] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [32589] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [32590] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409010.686719380] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.696301908] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.700265705] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.702719828] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.702736941] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.702741389] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.702980684] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.860846613] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.864053567] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.864502750] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.864580218] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.873333134] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874756919] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874796634] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874828325] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874835869] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874915590] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874969693] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409010.998711197] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.251069928] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.251140081] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.254010582] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.266969910] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.268098811] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.268206015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.277044884] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.277510007] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.280816815] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.281804592] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.281845991] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.287064242] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.298641395] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.299003282] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.299893428] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.311253639] [controller_manager]: Overrun might occur, Total time : 9947.300 us (Expected < 2000.000 us) --> Read time : 12.914 us, Update time : 9932.152 us (Switch time : 9888.690 us (Switch chained mode time : 0.691 us, perform mode change time : 10.560 us, Activation time : 9862.320 us, Deactivation time : 0.161 us)), Write time : 2.234 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.311326467] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.104770 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.311267836] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.314854471] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.315962932] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.316025521] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.316364194] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.326678598] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.328770634] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.329087426] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.329165674] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.332007584] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.333449334] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.336709729] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.337830128] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.337870545] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.338996394] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.356977304] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.357344462] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.359973292] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.362518594] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.363474275] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.366577315] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.367953771] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.367985101] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.368252769] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.385557587] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.385984272] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.390264464] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.391449962] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.394649252] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.396071629] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.396112647] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.396503649] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.411317257] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.425152336] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425514362] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425725223] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425781730] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425809663] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.428261043] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.436395799] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.437580595] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.440882559] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 32589] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409011.788984488] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.208551170] [controller_manager]: Overrun might occur, Total time : 3249.815 us (Expected < 2000.000 us) --> Read time : 2968.210 us, Update time : 278.930 us, Write time : 2.675 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409013.066150608] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.071854915] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.633579 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 32587] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.424310066] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.424372264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.543200365] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.543282021] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.543451092] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.545886393] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.664946 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.564706333] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566258499] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566402332] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566437689] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566453890] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.568179028] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.574716419] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.574742418] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.576399747] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.592742355] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.593038142] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.593972316] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.596952846] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.596975629] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.597811373] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.614667065] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.615405543] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.616021543] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.618713037] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.618735600] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.619651916] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.634685001] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.635574436] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.636260619] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.639338713] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.639358380] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.639811862] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.653444801] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.654268006] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.667567853] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.667605865] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.667918980] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.683011728] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.684173345] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.691723277] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.691764375] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.692469544] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.708776554] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.710233772] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.713331654] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.713365809] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.713633584] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.729014625] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.729872685] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 32590] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.546436975] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.215869 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.669720656] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.499179 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.054380047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.158400 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.348171367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.950251 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409020.937156000] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.937210133] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.379388443] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.167117 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409021.936779045] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.936830403] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409022.936727011] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409022.936786224] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409023.936929120] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409023.936983644] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409024.936780949] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409024.936831305] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409025.398858931] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409025.937238058] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409025.937301318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409025.949971093] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409026.518345970] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409026.518418778] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409026.936841666] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409026.936884748] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409027.936869374] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409027.936929287] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409028.936981644] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409028.937193731] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409029.936771943] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409029.936830324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409030.936850265] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409030.936925698] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409031.936824906] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409031.936883608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409032.936864678] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409032.936918370] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409033.936777707] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409033.936829736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409034.936901721] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409034.936955754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409035.936853648] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409035.936907590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409036.936811406] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409036.936864317] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.315860179] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409037.213346839] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:19 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:36751 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:36751 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.245905266] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.245968446] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.245982232] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.252387979] [moveit_2882792308.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.254749016] [moveit_2882792308.moveit.ros.rdf_loader]: Loaded robot model in 0.00209667 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.254777200] [moveit_2882792308.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.254784704] [moveit_2882792308.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409023.266574569] [moveit_2882792308.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.269300339] [moveit_2882792308.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.325565849] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.325708581] [moveit_2882792308.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.326718009] [moveit_2882792308.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327277251] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327291939] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327398286] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327900061] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327988309] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.328585322] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.328598828] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.329053752] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.329505531] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409023.329734806] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409023.329752169] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.488595075] [moveit_2882792308.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.489266010] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490406918] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490419261] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490752514] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490766451] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490790015] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490795185] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490810935] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.491639349] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.493935141] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.493950330] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.496156492] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.496170008] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.496809443] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.526223248] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.530075442] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.530245956] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.530271745] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.531111836] [moveit_2882792308.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:25] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:25] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:26 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547103323] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547236236] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547248579] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547267606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409026.547376312] [moveit_2882792308.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547832829] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547954100] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409036.548437353] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409036.548540139] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:37] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409037.342777967] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:52 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:52 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:52 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409052.485310461] [moveit_2882792308.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:12 + src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_overhead_obstacle_cylinder.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py::test_overhead_cylinder[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed +======================== 1 failed, 1 warning in 45.79s ========================= + + +12:31:26.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests +12:31:26.36 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-979Owj +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py::test_pick_up_mesh_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:31:07 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-54-148841-DESKTOP-HG3Q5KV-1338 (testutils.py:33) + +2026-05-10 12:31:07 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [1343] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [1346] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [1347] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-4]: process started with pid [1348] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-5]: process started with pid [1349] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [robot_state_publisher-2] [INFO] [1778409054.492759738] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.495224431] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.498820369] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.501125339] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.501146269] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.501150327] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409054.501249686] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.667833601] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.671788334] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.672213121] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.672298062] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.681897383] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683529694] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683571223] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683603124] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683609676] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683690940] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683753780] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409054.807033336] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.059384642] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.059443164] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.062480886] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.087157785] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088540412] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088769437] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088799965] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088826756] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.094806380] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.103418863] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.103471924] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.104823202] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.119556156] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.119886329] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.120792350] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.122915835] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.122949599] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.123662794] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.137034362] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.137388285] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.138036706] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.141342192] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.141380465] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.142238792] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.157029270] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.157362705] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.158371135] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.161467348] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.161533824] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.166797912] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.183353894] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.183754901] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.197198550] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.197250980] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.197982740] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409055.219639942] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.053708 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.223796851] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.224976583] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.237752654] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.237776309] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.238293887] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.274598431] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.275604468] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.290343208] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.290543589] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.292535022] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.318920026] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.328564897] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409055.467057560] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 1349] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409056.326183637] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.597142 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.720130567] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.720178959] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.721608926] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.736888114] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.737940624] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.738021437] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.743496477] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.743962607] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.746842989] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.748051155] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.748086953] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.748475121] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.762982196] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.763290462] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.764335137] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409056.774915572] [controller_manager]: Overrun might occur, Total time : 9221.967 us (Expected < 2000.000 us) --> Read time : 13.325 us, Update time : 9205.196 us (Switch time : 9148.860 us (Switch chained mode time : 0.671 us, perform mode change time : 12.794 us, Activation time : 9118.683 us, Deactivation time : 0.171 us)), Write time : 3.446 us (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.774923050] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.776705768] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.777821703] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.777858092] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.778247301] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.791045094] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.792700765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.793025366] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.793085401] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.796531434] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.797815334] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.800942480] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.802219622] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.802256011] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.803323420] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.820895143] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.821249606] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.823539132] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.826564470] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.827807072] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.830960053] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.832204317] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.832241097] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.832659933] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.851264710] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.851652811] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.854616910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.855810441] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.858838459] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.859963336] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.859998293] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.860455582] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.874767868] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.889223255] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889596389] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889741765] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889753567] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889762094] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.891435978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.898905166] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.900028285] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.903250598] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [robot_state_publisher-2] [INFO] [1778409056.925659248] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1346] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 1348] (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.250394589] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409057.250447649] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.162232778] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.646283 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.358299668] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.713674 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.214275760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.689455 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409063.777631938] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.777687103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409064.777955253] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409064.778007863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.777931584] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.777976690] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.112370298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.784094 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409066.778073384] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.778151713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [1778409057.113520576] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] [INFO] [1778409059.826449092] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:31:07 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:02 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] * Running on http://127.0.0.1:47197 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] * Running on http://172.30.43.138:47197 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.153084699] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.153117050] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.153124685] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.163130246] [moveit_1908157456.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.169450425] [moveit_1908157456.moveit.ros.rdf_loader]: Loaded robot model in 0.006204 seconds (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.169484340] [moveit_1908157456.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.169498136] [moveit_1908157456.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [WARN] [1778409066.188733316] [moveit_1908157456.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.192459536] [moveit_1908157456.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.302376041] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.302489857] [moveit_1908157456.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.310336176] [moveit_1908157456.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.310936526] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.310953959] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.311390459] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.311673897] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.311736586] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.312857286] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.312870090] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.313256515] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.313640334] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [WARN] [1778409066.313909546] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [ERROR] [1778409066.313931187] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.693934398] [moveit_1908157456.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.694821654] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696113855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696132530] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696366926] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696379670] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696398656] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696403275] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696417702] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.697249433] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.699675983] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.699691553] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.701725207] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.701739113] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.702581734] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.731130070] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.735219250] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.735407838] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.735430381] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.736174906] [moveit_1908157456.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:07] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:07] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409067.218353106] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:22 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:22 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:22 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:31:25 [ ERROR] [INFO] [1778409082.335691810] [moveit_1908157456.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +________________________ test_pick_up_mesh_by_id[ur5e] _________________________ + +start_processes = Urls(ros_domain_id='6', robot_url='http://0.0.0.0:47197', storage_url='http://127.0.0.1:49373') + + @pytest.mark.timeout(321) + def test_pick_up_mesh_by_id(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" +  + storage_client.URL = start_processes.storage_url +  + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) +  + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Mesh("Mesh1", MESH_ASSET_ID) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:42: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Mesh1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-54-148841-DESKTOP-HG3Q5KV-1338 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [1343] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [1346] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [1347] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [1348] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [1349] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409054.492759738] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.495224431] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.498820369] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.501125339] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.501146269] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.501150327] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409054.501249686] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.667833601] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.671788334] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.672213121] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.672298062] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.681897383] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683529694] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683571223] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683603124] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683609676] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683690940] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683753780] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409054.807033336] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.059384642] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.059443164] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.062480886] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.087157785] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088540412] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088769437] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088799965] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088826756] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.094806380] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.103418863] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.103471924] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.104823202] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.119556156] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.119886329] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.120792350] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.122915835] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.122949599] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.123662794] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.137034362] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.137388285] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.138036706] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.141342192] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.141380465] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.142238792] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.157029270] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.157362705] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.158371135] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.161467348] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.161533824] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.166797912] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.183353894] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.183754901] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.197198550] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.197250980] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.197982740] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409055.219639942] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.053708 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.223796851] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.224976583] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.237752654] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.237776309] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.238293887] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.274598431] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.275604468] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.290343208] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.290543589] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.292535022] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.318920026] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.328564897] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409055.467057560] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 1349] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409056.326183637] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.597142 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.720130567] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.720178959] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.721608926] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.736888114] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.737940624] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.738021437] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.743496477] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.743962607] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.746842989] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.748051155] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.748086953] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.748475121] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.762982196] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.763290462] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.764335137] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409056.774915572] [controller_manager]: Overrun might occur, Total time : 9221.967 us (Expected < 2000.000 us) --> Read time : 13.325 us, Update time : 9205.196 us (Switch time : 9148.860 us (Switch chained mode time : 0.671 us, perform mode change time : 12.794 us, Activation time : 9118.683 us, Deactivation time : 0.171 us)), Write time : 3.446 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.774923050] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.776705768] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.777821703] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.777858092] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.778247301] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.791045094] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.792700765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.793025366] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.793085401] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.796531434] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.797815334] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.800942480] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.802219622] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.802256011] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.803323420] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.820895143] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.821249606] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.823539132] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.826564470] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.827807072] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.830960053] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.832204317] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.832241097] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.832659933] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.851264710] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.851652811] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.854616910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.855810441] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.858838459] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.859963336] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.859998293] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.860455582] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.874767868] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.889223255] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889596389] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889741765] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889753567] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889762094] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.891435978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.898905166] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.900028285] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.903250598] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409056.925659248] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1346] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 1348] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.250394589] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409057.250447649] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.162232778] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.646283 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.358299668] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.713674 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.214275760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.689455 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409063.777631938] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.777687103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409064.777955253] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409064.778007863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.777931584] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.777976690] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.112370298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.784094 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409066.778073384] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.778151713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409057.113520576] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409059.826449092] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:47197 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:47197 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.153084699] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.153117050] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.153124685] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.163130246] [moveit_1908157456.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.169450425] [moveit_1908157456.moveit.ros.rdf_loader]: Loaded robot model in 0.006204 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.169484340] [moveit_1908157456.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.169498136] [moveit_1908157456.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409066.188733316] [moveit_1908157456.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.192459536] [moveit_1908157456.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.302376041] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.302489857] [moveit_1908157456.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.310336176] [moveit_1908157456.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.310936526] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.310953959] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.311390459] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.311673897] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.311736586] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.312857286] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.312870090] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.313256515] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.313640334] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409066.313909546] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409066.313931187] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.693934398] [moveit_1908157456.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.694821654] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696113855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696132530] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696366926] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696379670] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696398656] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696403275] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696417702] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.697249433] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.699675983] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.699691553] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.701725207] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.701739113] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.702581734] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.731130070] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.735219250] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.735407838] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.735430381] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.736174906] [moveit_1908157456.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:07] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:07] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409067.218353106] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409082.335691810] [moveit_1908157456.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:18 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_mesh_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py::test_pick_up_mesh_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 32.42s ========================= + + +12:32:05.12 [INFO] Long running tasks: + 69.28s Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests + 76.08s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests + 85.54s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:32:06.34 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests +12:32:06.34 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-8F8uhs +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py::test_side_mesh[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:31:47 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-57-056550-DESKTOP-HG3Q5KV-1587 (testutils.py:33) + +2026-05-10 12:31:47 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [1625] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [1626] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [1627] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-4]: process started with pid [1628] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-5]: process started with pid [1629] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [robot_state_publisher-2] [INFO] [1778409057.393905761] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.398271588] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.401531231] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.403677229] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.403700123] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.403705473] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409057.403846946] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.573432623] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.576825971] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.577266658] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.577349074] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.586116609] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587551216] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587585390] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587617542] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587624284] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587703435] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587760102] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409057.710115326] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.962415467] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.962489227] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.964955464] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409057.977448988] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.978520489] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.978641539] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.985778886] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.986405105] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409057.989159238] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.989878820] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.989907725] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.994642557] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.007232985] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.007513208] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.008885305] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409058.020060278] [controller_manager]: Overrun might occur, Total time : 9884.320 us (Expected < 2000.000 us) --> Read time : 20.448 us, Update time : 9861.057 us (Switch time : 9804.910 us (Switch chained mode time : 0.701 us, perform mode change time : 13.325 us, Activation time : 9772.639 us, Deactivation time : 0.241 us)), Write time : 2.815 us (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409058.020116365] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.034311 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.020066526] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.023476199] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.024476024] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.024539094] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.024963952] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.036027428] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.037506413] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.037764519] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.037820715] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.041107877] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.042292683] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.045288335] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.046203118] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.046239548] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.047357061] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.061734690] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.062020970] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.064305706] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.067072413] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.068302605] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.071378369] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.072343950] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.072363026] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.072693589] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.085240775] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.085493820] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.088854938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.090351356] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.093592744] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.094845620] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.094905764] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.095256531] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.106564243] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.119186711] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119467239] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119672369] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119725149] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119755817] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.122436821] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.129093284] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.130423642] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.133748255] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.255986147] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 1628] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.508711119] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.508770312] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.509235616] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.533349936] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534511864] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534640008] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534668832] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534683070] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.535924523] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.541650262] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.541688064] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.543106248] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.555128067] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.555394016] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.556107242] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.559578909] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.559614877] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.560297519] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.575131896] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.575369983] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.575958231] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.577621296] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.577655401] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.578396840] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.592978542] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.593216900] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.593747218] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.595627605] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.595663383] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.595948074] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.608997735] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.609283259] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.617497431] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.617550161] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.617781932] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.631246410] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.631508082] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.637922836] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.637990415] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.638375422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.653243147] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.653496293] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.657699070] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.657737984] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.658099762] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.673327731] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.673608484] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 1629] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.158212859] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.130965 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [robot_state_publisher-2] [INFO] [1778409059.826465709] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1626] (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.014038409] [controller_manager]: Overrun might occur, Total time : 7879.611 us (Expected < 2000.000 us) --> Read time : 16.792 us, Update time : 7857.720 us, Write time : 5.099 us (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409060.146723717] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.146776758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.358203966] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.122173 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.210785563] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.703469 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.112919441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.837598 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409066.636066602] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.636124843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409067.636477549] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409067.636526131] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409068.636422189] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409068.636468367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409069.414015023] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.932839 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409069.636516783] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409069.636569804] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409070.636505064] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409070.636560870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409071.397596444] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409071.636440927] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409071.636492324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409071.948646189] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409072.526736679] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.526791333] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.636558699] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409072.636609195] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.723430772] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.723510634] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409073.636365181] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409073.636420225] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409074.636178275] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409074.636241675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409075.636207945] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409075.636253812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409076.446235885] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409076.636207022] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409076.636246187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409077.636244745] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409077.636286685] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409078.636233287] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409078.636275667] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409079.636409575] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409079.636466954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409080.636140945] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409080.636191050] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409081.636366728] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409081.636428636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409082.636261368] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409082.636313246] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409083.636647711] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409083.636702325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409084.636333148] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409084.636369046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409085.636258635] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409085.636298361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.382266034] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.183810 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409086.636615062] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.636644398] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409087.636235435] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409087.636641607] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409088.636348004] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409088.636377460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409088.734197088] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.115265 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409089.636389653] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409089.636446601] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409089.977361987] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.386729955] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.647771 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.528256021] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409090.636392147] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.636450007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409091.104154587] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.104229640] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.145009856] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.145070281] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.636299305] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409091.636351965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409092.258234201] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409092.636510541] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.636565316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.690195175] [controller_manager]: Overrun might occur, Total time : 2050.141 us (Expected < 2000.000 us) --> Read time : 16.532 us, Update time : 2031.876 us, Write time : 1.733 us (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.690240521] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.158718 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409093.636391211] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.636449461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.898545401] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.463748 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409094.636317763] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409094.636370543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409095.636504934] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409095.636555720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.619007763] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.926000 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409096.638469374] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.638499200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409097.557072511] [controller_manager]: Overrun might occur, Total time : 2919.579 us (Expected < 2000.000 us) --> Read time : 14.267 us, Update time : 2903.739 us, Write time : 1.573 us (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409097.636409950] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409097.636462670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409098.636442343] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409098.636492508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409099.636344232] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409099.636407863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409099.961066696] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.984482 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409100.636464336] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409100.636520132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409101.410284316] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.202303 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409101.636283906] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409101.636325245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409102.636625025] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409102.636670882] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409103.636342079] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.636403837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.731012425] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930311 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.747636241] [controller_manager]: Overrun might occur, Total time : 2598.958 us (Expected < 2000.000 us) --> Read time : 25.048 us, Update time : 2571.415 us, Write time : 2.495 us (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409104.636463729] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409104.636516870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409105.636609049] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409105.636657461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409106.636902064] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409106.636959924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [1778409060.030988337] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] [INFO] [1778409090.383598995] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:31:47 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] * Running on http://127.0.0.1:56347 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] * Running on http://172.30.43.138:56347 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.249470348] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.249524581] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.249541453] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.256581019] [moveit_3227491728.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.258494535] [moveit_3227491728.moveit.ros.rdf_loader]: Loaded robot model in 0.00175904 seconds (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.258521055] [moveit_3227491728.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.258531044] [moveit_3227491728.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [WARN] [1778409069.269987853] [moveit_3227491728.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.272974759] [moveit_3227491728.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.325908069] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.326026614] [moveit_3227491728.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.326990465] [moveit_3227491728.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327486678] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327501637] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327611415] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327970518] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.328041683] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.328869781] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.328886433] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.329438131] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.329938833] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [WARN] [1778409069.330222959] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [ERROR] [1778409069.330247545] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.399311243] [moveit_3227491728.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.400248770] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401510396] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401525936] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401832624] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401846460] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401870175] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401875746] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401886727] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.402776557] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.405202514] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.405216600] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.407487981] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.407502739] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.408249692] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.782849453] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.786708094] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.786883868] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.786913144] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.787926139] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:11] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:11] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684143616] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684298691] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684330622] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684374645] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [WARN] [1778409072.684528999] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.685146251] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.685258144] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.717025656] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.719363518] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.719937599] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Executing plan (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720709100] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720754927] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720792619] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720842614] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720867050] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722670537] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722702167] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722745710] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722882450] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.723764370] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.723784989] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409076.474738864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409076.480470850] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:16] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:17] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:17] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:20] "PUT /collisions/mesh?meshId=Mesh2&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:20] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:23] "PUT /collisions/mesh?meshId=Mesh3&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:23] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:26] "PUT /collisions/mesh?meshId=Mesh4&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:26] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115116240] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115183888] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115194699] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115205920] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [WARN] [1778409091.115263620] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115541498] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115585311] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.141696641] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.141989909] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142309786] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Executing plan (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142632400] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142656085] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142675962] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142684829] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142705388] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144522625] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144556199] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144578572] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144696235] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.145302849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.145315924] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409092.295947666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409092.300474879] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:47 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self._wait_for_pose_reached(target_abs, tolerance=0.01) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] raise UrGeneral("Target pose was not reached.") (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] arcor2_ur.exceptions.UrGeneral: Target pose was not reached. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:47] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=true HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409107.521062780] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:32:02 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:32:02 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:32:02 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:32:05 [ ERROR] [INFO] [1778409122.678009905] [moveit_3227491728.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________________ test_side_mesh[ur5e] _____________________________ + +start_processes = Urls(ros_domain_id='88', robot_url='http://0.0.0.0:56347', storage_url='http://127.0.0.1:52313') + + @pytest.mark.timeout(400) + def test_side_mesh(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" +  + storage_client.URL = start_processes.storage_url +  + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) +  + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +  + ot.move_to_pose("", start_pose, 0.3, safe=False) + time.sleep(1) +  + mesh_positions = [ + # rgith + Position(0.5, 0.5, 0.25), + Position(0.5, 0.5, 0.55), + # left + Position(-0.5, 0.5, 0.25), + Position(-0.5, 0.5, 0.55), + ] +  + for idx, position in enumerate(mesh_positions, start=1): + mesh = Mesh(f"Mesh{idx}", MESH_ASSET_ID) +  + scene_service.upsert_collision( + mesh, + Pose(position, Orientation(0, 0, 0, 1)), + ) +  + assert mesh.id in scene_service.collision_ids() + time.sleep(3) +  +> ot.move_to_pose("", goal_pose, 0.3) + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:66: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not reached. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-57-056550-DESKTOP-HG3Q5KV-1587 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [1625] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [1626] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [1627] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [1628] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [1629] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409057.393905761] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.398271588] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.401531231] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.403677229] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.403700123] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.403705473] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409057.403846946] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.573432623] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.576825971] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.577266658] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.577349074] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.586116609] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587551216] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587585390] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587617542] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587624284] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587703435] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587760102] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409057.710115326] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.962415467] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.962489227] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.964955464] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409057.977448988] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.978520489] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.978641539] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.985778886] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.986405105] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409057.989159238] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.989878820] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.989907725] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.994642557] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.007232985] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.007513208] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.008885305] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409058.020060278] [controller_manager]: Overrun might occur, Total time : 9884.320 us (Expected < 2000.000 us) --> Read time : 20.448 us, Update time : 9861.057 us (Switch time : 9804.910 us (Switch chained mode time : 0.701 us, perform mode change time : 13.325 us, Activation time : 9772.639 us, Deactivation time : 0.241 us)), Write time : 2.815 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409058.020116365] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.034311 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.020066526] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.023476199] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.024476024] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.024539094] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.024963952] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.036027428] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.037506413] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.037764519] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.037820715] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.041107877] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.042292683] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.045288335] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.046203118] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.046239548] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.047357061] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.061734690] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.062020970] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.064305706] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.067072413] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.068302605] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.071378369] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.072343950] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.072363026] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.072693589] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.085240775] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.085493820] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.088854938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.090351356] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.093592744] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.094845620] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.094905764] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.095256531] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.106564243] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.119186711] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119467239] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119672369] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119725149] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119755817] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.122436821] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.129093284] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.130423642] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.133748255] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.255986147] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 1628] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.508711119] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.508770312] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.509235616] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.533349936] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534511864] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534640008] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534668832] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534683070] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.535924523] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.541650262] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.541688064] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.543106248] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.555128067] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.555394016] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.556107242] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.559578909] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.559614877] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.560297519] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.575131896] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.575369983] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.575958231] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.577621296] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.577655401] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.578396840] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.592978542] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.593216900] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.593747218] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.595627605] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.595663383] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.595948074] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.608997735] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.609283259] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.617497431] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.617550161] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.617781932] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.631246410] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.631508082] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.637922836] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.637990415] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.638375422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.653243147] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.653496293] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.657699070] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.657737984] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.658099762] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.673327731] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.673608484] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 1629] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.158212859] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.130965 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409059.826465709] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1626] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.014038409] [controller_manager]: Overrun might occur, Total time : 7879.611 us (Expected < 2000.000 us) --> Read time : 16.792 us, Update time : 7857.720 us, Write time : 5.099 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409060.146723717] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.146776758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.358203966] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.122173 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.210785563] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.703469 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.112919441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.837598 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409066.636066602] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.636124843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409067.636477549] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409067.636526131] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409068.636422189] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409068.636468367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409069.414015023] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.932839 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409069.636516783] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409069.636569804] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409070.636505064] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409070.636560870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409071.397596444] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409071.636440927] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409071.636492324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409071.948646189] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409072.526736679] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.526791333] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.636558699] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409072.636609195] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.723430772] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.723510634] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409073.636365181] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409073.636420225] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409074.636178275] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409074.636241675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409075.636207945] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409075.636253812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409076.446235885] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409076.636207022] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409076.636246187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409077.636244745] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409077.636286685] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409078.636233287] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409078.636275667] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409079.636409575] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409079.636466954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409080.636140945] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409080.636191050] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409081.636366728] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409081.636428636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409082.636261368] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409082.636313246] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409083.636647711] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409083.636702325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409084.636333148] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409084.636369046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409085.636258635] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409085.636298361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.382266034] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.183810 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409086.636615062] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.636644398] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409087.636235435] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409087.636641607] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409088.636348004] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409088.636377460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409088.734197088] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.115265 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409089.636389653] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409089.636446601] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409089.977361987] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.386729955] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.647771 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.528256021] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409090.636392147] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.636450007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409091.104154587] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.104229640] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.145009856] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.145070281] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.636299305] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409091.636351965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409092.258234201] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409092.636510541] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.636565316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.690195175] [controller_manager]: Overrun might occur, Total time : 2050.141 us (Expected < 2000.000 us) --> Read time : 16.532 us, Update time : 2031.876 us, Write time : 1.733 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.690240521] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.158718 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409093.636391211] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.636449461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.898545401] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.463748 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409094.636317763] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409094.636370543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409095.636504934] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409095.636555720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.619007763] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.926000 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409096.638469374] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.638499200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409097.557072511] [controller_manager]: Overrun might occur, Total time : 2919.579 us (Expected < 2000.000 us) --> Read time : 14.267 us, Update time : 2903.739 us, Write time : 1.573 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409097.636409950] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409097.636462670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409098.636442343] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409098.636492508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409099.636344232] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409099.636407863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409099.961066696] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.984482 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409100.636464336] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409100.636520132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409101.410284316] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.202303 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409101.636283906] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409101.636325245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409102.636625025] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409102.636670882] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409103.636342079] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.636403837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.731012425] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930311 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.747636241] [controller_manager]: Overrun might occur, Total time : 2598.958 us (Expected < 2000.000 us) --> Read time : 25.048 us, Update time : 2571.415 us, Write time : 2.495 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409104.636463729] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409104.636516870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409105.636609049] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409105.636657461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409106.636902064] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409106.636959924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409060.030988337] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409090.383598995] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:56347 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:56347 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.249470348] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.249524581] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.249541453] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.256581019] [moveit_3227491728.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.258494535] [moveit_3227491728.moveit.ros.rdf_loader]: Loaded robot model in 0.00175904 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.258521055] [moveit_3227491728.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.258531044] [moveit_3227491728.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409069.269987853] [moveit_3227491728.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.272974759] [moveit_3227491728.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.325908069] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.326026614] [moveit_3227491728.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.326990465] [moveit_3227491728.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327486678] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327501637] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327611415] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327970518] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.328041683] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.328869781] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.328886433] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.329438131] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.329938833] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409069.330222959] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409069.330247545] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.399311243] [moveit_3227491728.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.400248770] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401510396] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401525936] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401832624] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401846460] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401870175] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401875746] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401886727] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.402776557] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.405202514] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.405216600] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.407487981] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.407502739] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.408249692] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.782849453] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.786708094] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.786883868] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.786913144] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.787926139] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:11] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:11] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684143616] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684298691] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684330622] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684374645] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409072.684528999] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.685146251] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.685258144] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.717025656] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.719363518] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.719937599] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720709100] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720754927] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720792619] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720842614] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720867050] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722670537] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722702167] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722745710] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722882450] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.723764370] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.723784989] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409076.474738864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409076.480470850] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:16] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:17] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:17] "GET /collisions HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:20] "PUT /collisions/mesh?meshId=Mesh2&meshFileId=triangle_block.stl HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:20] "GET /collisions HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:23] "PUT /collisions/mesh?meshId=Mesh3&meshFileId=triangle_block.stl HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:23] "GET /collisions HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:26] "PUT /collisions/mesh?meshId=Mesh4&meshFileId=triangle_block.stl HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:26] "GET /collisions HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115116240] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115183888] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115194699] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115205920] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409091.115263620] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115541498] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115585311] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.141696641] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.141989909] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142309786] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142632400] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142656085] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142675962] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142684829] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142705388] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144522625] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144556199] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144578572] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144696235] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.145302849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.145315924] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409092.295947666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409092.300474879] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:47 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._wait_for_pose_reached(target_abs, tolerance=0.01) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Target pose was not reached.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Target pose was not reached. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:47] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=true HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409107.521062780] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:02 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:02 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:02 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409122.678009905] [moveit_3227491728.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:17 + src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:17: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_mesh.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py::test_side_mesh[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not ... +=================== 1 failed, 1 warning in 69.45s (0:01:09) ==================== + + +12:32:11.98 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests +12:32:11.99 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-Lj2y79 +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py::test_pick_and_place_mesh_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:31:52 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-50-208901-DESKTOP-HG3Q5KV-1093 (testutils.py:33) + +2026-05-10 12:31:52 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [1108] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [1109] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [1110] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-4]: process started with pid [1111] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-5]: process started with pid [1112] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409050.534796294] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.537717464] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.539797397] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.541434227] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.541465627] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.541472660] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409050.541572230] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409050.826096610] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.881925906] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.884237448] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.884584057] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.884652186] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.892684554] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893460167] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893475166] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893495414] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893499502] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893554296] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893585987] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.078451158] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.078511332] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.081136871] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.103279230] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.104810183] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.105073624] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.105125582] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.105155148] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.110051927] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.117058706] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.117100085] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.117948066] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.131007942] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.131281597] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.132131061] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.135425550] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.135471017] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.136281983] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.149171840] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.149438878] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.150040421] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.153769461] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.153841478] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.154597345] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.169772434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.170196635] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.170745598] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.173786576] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.173830579] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.177773808] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.190955349] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.191269275] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.201557575] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.201600797] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.201947716] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.219079168] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.219347022] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.225294273] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.225352233] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.225690160] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.239257684] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.239552910] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.243234370] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.243277932] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.243831860] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.259697979] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.260074338] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.396575070] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 1112] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.649298425] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.649350063] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.650571073] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.667248592] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.668532653] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.668628866] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.673671282] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.674127448] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.677083259] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.678549010] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.678591761] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.679071757] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.695225944] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.695591267] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.696701116] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409051.709291935] [controller_manager]: Overrun might occur, Total time : 11376.546 us (Expected < 2000.000 us) --> Read time : 13.235 us, Update time : 11360.065 us (Switch time : 11312.333 us (Switch chained mode time : 0.421 us, perform mode change time : 14.718 us, Activation time : 11280.353 us, Deactivation time : 0.180 us)), Write time : 3.246 us (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409051.709347380] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.520978 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.709308296] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.710980593] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.712138253] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.712199029] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.712583239] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.727533787] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.729140805] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.729507187] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.729569315] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.732760378] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.734021689] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.736963655] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.738046052] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.738084866] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.739318594] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.759246234] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.759640443] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.761893724] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.764864128] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.766030391] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.768986255] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.770436371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.770476868] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.770956344] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.789394509] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.789769527] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.792833303] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.794043797] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.797385648] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.798823245] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.798891695] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.799318336] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.814566600] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.829250391] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.829801739] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.830022047] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.830045101] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.830059328] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.831885137] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.841167424] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.842297711] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.845047848] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 1111] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409052.940765860] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1109] (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409053.125566390] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.740318 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409053.331487855] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409053.331539724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409056.062209410] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.383449 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.162219974] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.393683 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409059.762061294] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.762094558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.358231412] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.405491 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409060.673485740] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.673540484] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409061.673384822] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409061.673439777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409062.062225156] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.399055 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409062.673376527] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409062.673427474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.214282604] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.456503 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409063.673473817] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.673525916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409064.467116066] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409064.673560281] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409064.673611989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.018151099] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.673554340] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.673606920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.708278545] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.708316918] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.800995232] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.801050257] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.118272026] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.446015 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.148850314] [controller_manager]: Overrun might occur, Total time : 4929.837 us (Expected < 2000.000 us) --> Read time : 330.638 us, Update time : 4597.316 us, Write time : 1.883 us (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409066.673536295] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.673608743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409067.315934628] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409067.673511368] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409067.673559750] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409068.673245813] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409068.673305376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409069.673284334] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409069.673333839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409070.673452288] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409070.673525267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409071.673328758] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409071.673378513] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.673503119] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409072.673556400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409073.673474053] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409073.673525290] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409074.673346934] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409074.673390227] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409075.673389485] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409075.673437286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409076.673370617] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409076.673413779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409077.673247479] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409077.673294198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409078.673365653] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409078.673416199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409079.673473622] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409079.673527384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409080.673241165] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409080.673287714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409081.673360887] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409081.673410240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409082.673613631] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409082.673665219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409083.673528594] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409083.673583809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409084.673610609] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409084.673659832] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409085.673233739] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409085.673281590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.620302547] [controller_manager]: Overrun might occur, Total time : 2454.604 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2437.962 us, Write time : 1.523 us (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.620345719] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.519497 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409086.674855954] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.674890309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409087.673349401] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409087.673396140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409088.673695528] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409088.673753819] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409089.673320681] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409089.673367941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409090.673293690] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.673342302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.673300202] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409091.673350608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.660275078] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.448827 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409092.675720485] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.675749481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409093.673633496] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.673688570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.845895244] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.068972 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.959743805] [controller_manager]: Overrun might occur, Total time : 3776.131 us (Expected < 2000.000 us) --> Read time : 14.688 us, Update time : 3759.659 us, Write time : 1.784 us (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409094.673440575] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409094.673487163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409095.673717343] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409095.673778779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.619157513] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.331612 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.624662428] [controller_manager]: Overrun might occur, Total time : 4756.027 us (Expected < 2000.000 us) --> Read time : 15.330 us, Update time : 4738.834 us, Write time : 1.863 us (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409096.673973175] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.674008051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409097.673698728] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409097.673753683] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409098.673411690] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409098.673470191] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409099.673576313] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409099.673646887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409100.673355515] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409100.673404558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409101.673561053] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409101.673613252] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409102.673568321] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409102.673615831] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409103.673498821] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.673552864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.749625385] [controller_manager]: Overrun might occur, Total time : 4545.581 us (Expected < 2000.000 us) --> Read time : 19.046 us, Update time : 4523.880 us, Write time : 2.655 us (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.754227866] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.400994 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409104.673332985] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409104.673379834] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409105.673497858] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409105.673543965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409106.673637250] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409106.673696984] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409107.673716033] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409107.673767992] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409108.673330679] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409108.673375765] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409109.673348939] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409109.673401128] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409110.673397454] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409110.673450174] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409111.673368327] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409111.673421348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409112.673303607] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409112.673368180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [1778409053.205450453] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] [INFO] [1778409056.925656221] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:31:52 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:30:58 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] * Running on http://127.0.0.1:35607 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] * Running on http://172.30.43.138:35607 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:02 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.042274388] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.042334121] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.042348970] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.048385659] [moveit_2243744926.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.050200017] [moveit_2243744926.moveit.ros.rdf_loader]: Loaded robot model in 0.00165471 seconds (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.050239682] [moveit_2243744926.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.050261754] [moveit_2243744926.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [WARN] [1778409063.062221408] [moveit_2243744926.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.065425446] [moveit_2243744926.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.128810330] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.128956397] [moveit_2243744926.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130085512] [moveit_2243744926.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130662589] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130678409] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130828108] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131274186] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131360430] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131730994] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131747486] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.132953703] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.133378611] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [WARN] [1778409063.133619439] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [ERROR] [1778409063.133635269] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.203732351] [moveit_2243744926.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.204606873] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206344545] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206362569] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206650987] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206664232] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206687787] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206693207] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206704439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.207390292] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.209663841] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.209677668] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.212455336] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.212470064] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.213385363] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.244085982] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.258667178] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.258946078] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.258995763] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.263513246] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:03] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  Generated 1 grasp options for object Mesh1. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  IK accepted: 1 grasp options. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768116709] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768277775] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768316679] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768355032] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [WARN] [1778409065.768426778] [moveit_2243744926.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768893816] [moveit_2243744926.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.769003915] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.785176689] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.785656140] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.785925552] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Executing plan (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798546545] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798572955] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798595648] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798630495] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798651505] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800235478] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800274182] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800332112] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800473069] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.801281791] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.801313110] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409067.352128756] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409067.358165727] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:22 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:52 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:52] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409112.955506467] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:32:07 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:32:07 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:32:08 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:32:11 [ ERROR] [INFO] [1778409128.127482900] [moveit_2243744926.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +__________________ test_pick_and_place_mesh_by_position[ur5e] __________________ + +start_processes = Urls(ros_domain_id='24', robot_url='http://0.0.0.0:35607', storage_url='http://127.0.0.1:39361') + + @pytest.mark.timeout(321) + def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" +  + storage_client.URL = start_processes.storage_url +  + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) +  + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:45: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Mesh1. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-50-208901-DESKTOP-HG3Q5KV-1093 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [1108] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [1109] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [1110] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [1111] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [1112] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409050.534796294] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.537717464] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.539797397] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.541434227] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.541465627] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.541472660] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409050.541572230] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409050.826096610] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.881925906] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.884237448] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.884584057] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.884652186] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.892684554] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893460167] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893475166] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893495414] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893499502] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893554296] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893585987] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.078451158] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.078511332] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.081136871] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.103279230] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.104810183] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.105073624] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.105125582] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.105155148] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.110051927] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.117058706] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.117100085] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.117948066] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.131007942] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.131281597] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.132131061] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.135425550] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.135471017] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.136281983] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.149171840] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.149438878] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.150040421] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.153769461] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.153841478] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.154597345] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.169772434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.170196635] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.170745598] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.173786576] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.173830579] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.177773808] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.190955349] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.191269275] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.201557575] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.201600797] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.201947716] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.219079168] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.219347022] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.225294273] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.225352233] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.225690160] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.239257684] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.239552910] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.243234370] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.243277932] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.243831860] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.259697979] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.260074338] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.396575070] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 1112] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.649298425] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.649350063] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.650571073] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.667248592] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.668532653] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.668628866] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.673671282] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.674127448] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.677083259] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.678549010] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.678591761] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.679071757] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.695225944] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.695591267] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.696701116] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409051.709291935] [controller_manager]: Overrun might occur, Total time : 11376.546 us (Expected < 2000.000 us) --> Read time : 13.235 us, Update time : 11360.065 us (Switch time : 11312.333 us (Switch chained mode time : 0.421 us, perform mode change time : 14.718 us, Activation time : 11280.353 us, Deactivation time : 0.180 us)), Write time : 3.246 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409051.709347380] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.520978 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.709308296] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.710980593] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.712138253] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.712199029] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.712583239] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.727533787] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.729140805] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.729507187] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.729569315] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.732760378] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.734021689] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.736963655] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.738046052] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.738084866] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.739318594] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.759246234] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.759640443] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.761893724] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.764864128] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.766030391] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.768986255] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.770436371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.770476868] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.770956344] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.789394509] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.789769527] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.792833303] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.794043797] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.797385648] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.798823245] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.798891695] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.799318336] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.814566600] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.829250391] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.829801739] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.830022047] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.830045101] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.830059328] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.831885137] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.841167424] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.842297711] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.845047848] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 1111] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409052.940765860] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1109] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409053.125566390] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.740318 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409053.331487855] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409053.331539724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409056.062209410] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.383449 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.162219974] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.393683 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409059.762061294] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.762094558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.358231412] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.405491 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409060.673485740] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.673540484] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409061.673384822] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409061.673439777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409062.062225156] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.399055 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409062.673376527] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409062.673427474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.214282604] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.456503 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409063.673473817] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.673525916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409064.467116066] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409064.673560281] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409064.673611989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.018151099] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.673554340] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.673606920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.708278545] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.708316918] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.800995232] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.801050257] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.118272026] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.446015 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.148850314] [controller_manager]: Overrun might occur, Total time : 4929.837 us (Expected < 2000.000 us) --> Read time : 330.638 us, Update time : 4597.316 us, Write time : 1.883 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409066.673536295] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.673608743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409067.315934628] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409067.673511368] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409067.673559750] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409068.673245813] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409068.673305376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409069.673284334] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409069.673333839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409070.673452288] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409070.673525267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409071.673328758] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409071.673378513] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.673503119] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409072.673556400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409073.673474053] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409073.673525290] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409074.673346934] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409074.673390227] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409075.673389485] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409075.673437286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409076.673370617] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409076.673413779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409077.673247479] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409077.673294198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409078.673365653] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409078.673416199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409079.673473622] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409079.673527384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409080.673241165] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409080.673287714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409081.673360887] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409081.673410240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409082.673613631] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409082.673665219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409083.673528594] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409083.673583809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409084.673610609] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409084.673659832] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409085.673233739] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409085.673281590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.620302547] [controller_manager]: Overrun might occur, Total time : 2454.604 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2437.962 us, Write time : 1.523 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.620345719] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.519497 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409086.674855954] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.674890309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409087.673349401] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409087.673396140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409088.673695528] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409088.673753819] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409089.673320681] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409089.673367941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409090.673293690] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.673342302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.673300202] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409091.673350608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.660275078] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.448827 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409092.675720485] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.675749481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409093.673633496] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.673688570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.845895244] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.068972 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.959743805] [controller_manager]: Overrun might occur, Total time : 3776.131 us (Expected < 2000.000 us) --> Read time : 14.688 us, Update time : 3759.659 us, Write time : 1.784 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409094.673440575] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409094.673487163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409095.673717343] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409095.673778779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.619157513] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.331612 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.624662428] [controller_manager]: Overrun might occur, Total time : 4756.027 us (Expected < 2000.000 us) --> Read time : 15.330 us, Update time : 4738.834 us, Write time : 1.863 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409096.673973175] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.674008051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409097.673698728] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409097.673753683] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409098.673411690] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409098.673470191] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409099.673576313] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409099.673646887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409100.673355515] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409100.673404558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409101.673561053] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409101.673613252] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409102.673568321] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409102.673615831] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409103.673498821] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.673552864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.749625385] [controller_manager]: Overrun might occur, Total time : 4545.581 us (Expected < 2000.000 us) --> Read time : 19.046 us, Update time : 4523.880 us, Write time : 2.655 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.754227866] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.400994 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409104.673332985] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409104.673379834] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409105.673497858] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409105.673543965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409106.673637250] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409106.673696984] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409107.673716033] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409107.673767992] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409108.673330679] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409108.673375765] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409109.673348939] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409109.673401128] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409110.673397454] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409110.673450174] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409111.673368327] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409111.673421348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409112.673303607] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409112.673368180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409053.205450453] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409056.925656221] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:58 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:35607 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:35607 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.042274388] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.042334121] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.042348970] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.048385659] [moveit_2243744926.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.050200017] [moveit_2243744926.moveit.ros.rdf_loader]: Loaded robot model in 0.00165471 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.050239682] [moveit_2243744926.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.050261754] [moveit_2243744926.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409063.062221408] [moveit_2243744926.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.065425446] [moveit_2243744926.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.128810330] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.128956397] [moveit_2243744926.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130085512] [moveit_2243744926.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130662589] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130678409] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130828108] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131274186] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131360430] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131730994] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131747486] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.132953703] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.133378611] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409063.133619439] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409063.133635269] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.203732351] [moveit_2243744926.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.204606873] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206344545] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206362569] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206650987] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206664232] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206687787] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206693207] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206704439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.207390292] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.209663841] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.209677668] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.212455336] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.212470064] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.213385363] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.244085982] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.258667178] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.258946078] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.258995763] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.263513246] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:03] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  Generated 1 grasp options for object Mesh1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  IK accepted: 1 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768116709] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768277775] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768316679] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768355032] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409065.768426778] [moveit_2243744926.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768893816] [moveit_2243744926.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.769003915] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.785176689] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.785656140] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.785925552] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798546545] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798572955] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798595648] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798630495] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798651505] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800235478] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800274182] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800332112] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800473069] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.801281791] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.801313110] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409067.352128756] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409067.358165727] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:52 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:52] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409112.955506467] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:07 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:07 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:08 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409128.127482900] [moveit_2243744926.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:18 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_mesh_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py::test_pick_and_place_mesh_by_position[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... +=================== 1 failed, 1 warning in 82.06s (0:01:22) ==================== + + +12:32:35.18 [INFO] Long running tasks: + 68.82s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 115.60s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:32:38.20 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests +12:32:38.20 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-TEAde1 +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py::test_pick_up_cylinder_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:32:19 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-07-640294-DESKTOP-HG3Q5KV-3052 (testutils.py:33) + +2026-05-10 12:32:19 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [3056] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [3057] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [3058] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-4]: process started with pid [3059] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-5]: process started with pid [3060] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [robot_state_publisher-2] [INFO] [1778409127.993664675] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409127.996944521] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.000574068] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.002839793] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.002858950] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.002864220] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409128.002962276] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.181365313] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.185645682] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.186153106] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.186215374] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.194459603] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196573390] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196611933] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196639596] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196646229] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196725139] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196799610] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.344241989] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409128.519749112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.503087 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.678025591] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.678093881] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.681192939] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.694936324] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.696114086] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.696314838] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.707767143] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.709626737] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.713051289] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.714370847] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.714435009] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.720916324] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.733456382] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.733849429] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.736309753] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409128.751167685] [controller_manager]: Overrun might occur, Total time : 13823.967 us (Expected < 2000.000 us) --> Read time : 20.018 us, Update time : 13799.000 us (Switch time : 13740.969 us (Switch chained mode time : 0.441 us, perform mode change time : 15.489 us, Activation time : 13706.855 us, Deactivation time : 0.491 us)), Write time : 4.949 us (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.751190203] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.752958608] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.753963963] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.754038745] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.754311483] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.764250954] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.767190920] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.767588205] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.767709786] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.770711971] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.771464371] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.774687324] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.776033723] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.776058350] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.777694970] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.792778190] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.793083204] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.795951489] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.798880154] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.799518321] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.802756815] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.804679343] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.804706885] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.804946751] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.817030312] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.818030457] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.820535522] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.821497990] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.825206983] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.826818400] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.826891930] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.827192852] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.837350557] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.848832494] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849504902] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849781107] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849812507] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849838216] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.853181318] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.862805651] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.863711833] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.867325184] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409129.005576102] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 3059] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.260059673] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.260113485] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.260787582] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.284964681] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368481357] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368722636] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368764676] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368781447] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.371023768] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.377663194] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.377726424] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.379264296] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.396826636] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.397933018] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.399216362] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.404014219] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.404059425] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.405038495] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [robot_state_publisher-2] [INFO] [1778409130.409930156] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.425512078] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.426795136] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.427700386] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.431824462] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.431866121] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.432687893] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.449164459] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.449616384] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.450960872] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409130.453804823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.558878 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.456153660] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.456179930] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.456369480] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.471233203] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.473173439] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.484151273] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.484200247] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.484707255] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.502925638] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.503308690] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.511922644] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.511970775] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.512378129] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3057] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.534818142] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.535604206] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.539977866] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.540009266] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.540606385] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.559346830] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.559769223] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 3060] (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409131.686666050] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409131.686748396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409132.000946839] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.701065 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409133.877767277] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.521623 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.625747192] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.501367 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409137.102871533] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409137.102919725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409138.103064299] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.103116037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.291325368] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.079613 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409139.103057385] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.103107089] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.370623757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.377642 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [1778409130.689048478] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] [INFO] [1778409136.042658337] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:32:19 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] * Running on http://127.0.0.1:60489 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] * Running on http://172.30.43.138:60489 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.366806514] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.366867329] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.366882588] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.374051630] [moveit_2313487515.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.376068894] [moveit_2313487515.moveit.ros.rdf_loader]: Loaded robot model in 0.00183629 seconds (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.376110854] [moveit_2313487515.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.376121013] [moveit_2313487515.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [WARN] [1778409139.388832181] [moveit_2313487515.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.391833093] [moveit_2313487515.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.449507571] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.449650462] [moveit_2313487515.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.450663467] [moveit_2313487515.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451194186] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451209244] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451356975] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451800528] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451899025] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.452779328] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.452794878] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.453258188] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.453767276] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [WARN] [1778409139.454022841] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [ERROR] [1778409139.454063078] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.490527964] [moveit_2313487515.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.491455156] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.492869248] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.492886631] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493072474] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493085269] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493106850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493111198] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493121217] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493867605] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.502151326] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.502165202] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.505305559] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.505319525] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.506323733] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.536448119] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.540276398] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.540541912] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.540566309] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.541600113] [moveit_2313487515.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:19] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:19] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.767143883] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:34 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:34 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:34 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:32:37 [ ERROR] [INFO] [1778409154.888994241] [moveit_2313487515.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +______________________ test_pick_up_cylinder_by_id[ur5e] _______________________ + +start_processes = Urls(ros_domain_id='223', robot_url='http://0.0.0.0:60489', storage_url='http://127.0.0.1:55293') + + @pytest.mark.timeout(321) + def test_pick_up_cylinder_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Cylinder("Cyl1", 0.1, 0.2) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Cyl1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-07-640294-DESKTOP-HG3Q5KV-3052 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [3056] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [3057] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [3058] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [3059] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [3060] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409127.993664675] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409127.996944521] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.000574068] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.002839793] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.002858950] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.002864220] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409128.002962276] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.181365313] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.185645682] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.186153106] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.186215374] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.194459603] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196573390] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196611933] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196639596] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196646229] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196725139] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196799610] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.344241989] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409128.519749112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.503087 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.678025591] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.678093881] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.681192939] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.694936324] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.696114086] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.696314838] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.707767143] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.709626737] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.713051289] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.714370847] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.714435009] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.720916324] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.733456382] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.733849429] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.736309753] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409128.751167685] [controller_manager]: Overrun might occur, Total time : 13823.967 us (Expected < 2000.000 us) --> Read time : 20.018 us, Update time : 13799.000 us (Switch time : 13740.969 us (Switch chained mode time : 0.441 us, perform mode change time : 15.489 us, Activation time : 13706.855 us, Deactivation time : 0.491 us)), Write time : 4.949 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.751190203] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.752958608] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.753963963] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.754038745] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.754311483] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.764250954] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.767190920] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.767588205] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.767709786] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.770711971] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.771464371] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.774687324] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.776033723] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.776058350] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.777694970] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.792778190] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.793083204] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.795951489] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.798880154] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.799518321] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.802756815] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.804679343] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.804706885] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.804946751] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.817030312] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.818030457] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.820535522] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.821497990] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.825206983] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.826818400] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.826891930] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.827192852] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.837350557] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.848832494] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849504902] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849781107] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849812507] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849838216] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.853181318] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.862805651] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.863711833] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.867325184] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409129.005576102] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 3059] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.260059673] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.260113485] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.260787582] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.284964681] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368481357] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368722636] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368764676] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368781447] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.371023768] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.377663194] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.377726424] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.379264296] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.396826636] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.397933018] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.399216362] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.404014219] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.404059425] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.405038495] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409130.409930156] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.425512078] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.426795136] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.427700386] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.431824462] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.431866121] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.432687893] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.449164459] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.449616384] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.450960872] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409130.453804823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.558878 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.456153660] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.456179930] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.456369480] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.471233203] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.473173439] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.484151273] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.484200247] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.484707255] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.502925638] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.503308690] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.511922644] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.511970775] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.512378129] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3057] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.534818142] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.535604206] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.539977866] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.540009266] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.540606385] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.559346830] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.559769223] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 3060] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409131.686666050] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409131.686748396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409132.000946839] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.701065 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409133.877767277] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.521623 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.625747192] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.501367 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409137.102871533] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409137.102919725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409138.103064299] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.103116037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.291325368] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.079613 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409139.103057385] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.103107089] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.370623757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.377642 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409130.689048478] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409136.042658337] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:60489 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:60489 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.366806514] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.366867329] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.366882588] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.374051630] [moveit_2313487515.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.376068894] [moveit_2313487515.moveit.ros.rdf_loader]: Loaded robot model in 0.00183629 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.376110854] [moveit_2313487515.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.376121013] [moveit_2313487515.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409139.388832181] [moveit_2313487515.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.391833093] [moveit_2313487515.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.449507571] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.449650462] [moveit_2313487515.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.450663467] [moveit_2313487515.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451194186] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451209244] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451356975] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451800528] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451899025] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.452779328] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.452794878] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.453258188] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.453767276] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409139.454022841] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409139.454063078] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.490527964] [moveit_2313487515.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.491455156] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.492869248] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.492886631] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493072474] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493085269] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493106850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493111198] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493121217] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493867605] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.502151326] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.502165202] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.505305559] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.505319525] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.506323733] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.536448119] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.540276398] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.540541912] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.540566309] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.541600113] [moveit_2313487515.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:19] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:19] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.767143883] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:34 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:34 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:34 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409154.888994241] [moveit_2313487515.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:13 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_cylinder_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py::test_pick_up_cylinder_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 30.79s ========================= + + +12:32:45.72 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests +12:32:45.72 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-vnkI57 +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py::test_pick_up_cylinder_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:32:27 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-13-274212-DESKTOP-HG3Q5KV-3295 (testutils.py:33) + +2026-05-10 12:32:27 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [3309] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [3310] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [3311] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-4]: process started with pid [3312] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-5]: process started with pid [3313] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [robot_state_publisher-2] [INFO] [1778409133.616791812] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.620849729] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.626093281] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.629116406] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.629160610] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.629167643] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409133.629293988] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.792805024] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.795701517] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.796111216] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.796195686] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.805688448] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807031199] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807070965] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807102204] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807108697] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807211662] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807270845] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409133.876987100] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.925254 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409133.962259061] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.215213049] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.215320754] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.218649739] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.239469781] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240405047] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240640404] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240705758] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240734513] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.246172402] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.253283102] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.253322657] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.254378934] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.265071426] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.265334983] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.266198774] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.269233359] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.269286170] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.270174828] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.283118157] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.283457717] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.284038922] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.285851792] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.285892610] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.286579505] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.299158882] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.299602024] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.300175403] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.303461813] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.303501508] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.307538799] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.321246109] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.321600027] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.332091813] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.332152017] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.332474129] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.347624627] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.347992270] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.353576737] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.353617995] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.353902346] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.367456520] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.367776342] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.371653910] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.371689718] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.372203580] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.385205807] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.385518926] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.509216011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 3313] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.761818659] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.761874085] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.763153055] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.779318682] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.780765777] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.780855036] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.786164915] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.788401190] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.791675356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.792764486] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.792809050] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.793171729] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.809111437] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.809467328] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.811063666] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409134.822501084] [controller_manager]: Overrun might occur, Total time : 10357.445 us (Expected < 2000.000 us) --> Read time : 17.353 us, Update time : 10335.984 us (Switch time : 10280.810 us (Switch chained mode time : 0.481 us, perform mode change time : 16.201 us, Activation time : 10247.947 us, Deactivation time : 0.250 us)), Write time : 4.108 us (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.822524744] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.825671342] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.826861334] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.826896611] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.827403560] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.840998085] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.843664427] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.844058370] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.844151497] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.846907254] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.848256608] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.851081740] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.852289982] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.852328054] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.853566097] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.871777795] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.872079714] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.874172155] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.877245365] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.878373352] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.881642650] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.883008720] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.883041903] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.883441232] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.901294841] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.901712810] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.905153839] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.906336226] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.909352647] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.910869614] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.910908127] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.911244697] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.925152314] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.939485053] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940198287] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940385423] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940402115] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940409920] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.942166191] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.949263162] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.950472304] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.953202953] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 3312] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [robot_state_publisher-2] [INFO] [1778409136.042665587] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3310] (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409136.344135290] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.344194302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.780264713] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.203308 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.806188164] [controller_manager]: Overrun might occur, Total time : 4055.797 us (Expected < 2000.000 us) --> Read time : 55.586 us, Update time : 3998.387 us, Write time : 1.824 us (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.291528799] [controller_manager]: Overrun might occur, Total time : 3372.678 us (Expected < 2000.000 us) --> Read time : 29.486 us, Update time : 3341.459 us, Write time : 1.733 us (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.291563585] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.502340 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.497881903] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.820507 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.612694550] [controller_manager]: Overrun might occur, Total time : 4574.598 us (Expected < 2000.000 us) --> Read time : 13.847 us, Update time : 4559.078 us, Write time : 1.673 us (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409142.320743429] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.682124 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409142.728495692] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409142.728555245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409143.145050259] [controller_manager]: Overrun might occur, Total time : 4905.335 us (Expected < 2000.000 us) --> Read time : 14.808 us, Update time : 4888.733 us, Write time : 1.794 us (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409143.728655558] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409143.728704812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409144.728891890] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409144.728940272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409145.728664622] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409145.728713004] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409146.728705568] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409146.728752116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [1778409136.222518308] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] [INFO] [1778409147.353295221] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:32:27 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:21 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] * Running on http://127.0.0.1:52163 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] * Running on http://172.30.43.138:52163 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.045365582] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.045414214] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.045425586] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.050496542] [moveit_2895436883.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.052393977] [moveit_2895436883.moveit.ros.rdf_loader]: Loaded robot model in 0.00177331 seconds (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.052427250] [moveit_2895436883.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.052437039] [moveit_2895436883.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [WARN] [1778409145.064034325] [moveit_2895436883.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.066741860] [moveit_2895436883.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.124119435] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.124314852] [moveit_2895436883.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125197758] [moveit_2895436883.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125610662] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125625721] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125751275] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126111399] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126196000] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126719174] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126734153] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.127069861] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.127497764] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [WARN] [1778409145.127776724] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [ERROR] [1778409145.127793025] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.632426495] [moveit_2895436883.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.633152575] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634414122] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634428870] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634775629] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634791399] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634815064] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634824191] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634837777] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.635684866] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.638001489] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.638015806] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.640038239] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.640053417] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.640822338] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.667587055] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.671061547] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.671220008] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.671241970] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.672056778] [moveit_2895436883.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:27] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:27] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409147.484945837] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:42 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:42 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:42 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:32:45 [ ERROR] [INFO] [1778409162.645452794] [moveit_2895436883.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +___________________ test_pick_up_cylinder_by_position[ur5e] ____________________ + +start_processes = Urls(ros_domain_id='54', robot_url='http://0.0.0.0:52163', storage_url='http://127.0.0.1:51465') + + @pytest.mark.timeout(321) + def test_pick_up_cylinder_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Cylinder("Cyl1", 0.1, 0.2) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Cyl1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-13-274212-DESKTOP-HG3Q5KV-3295 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [3309] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [3310] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [3311] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [3312] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [3313] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409133.616791812] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.620849729] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.626093281] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.629116406] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.629160610] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.629167643] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409133.629293988] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.792805024] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.795701517] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.796111216] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.796195686] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.805688448] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807031199] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807070965] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807102204] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807108697] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807211662] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807270845] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409133.876987100] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.925254 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409133.962259061] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.215213049] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.215320754] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.218649739] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.239469781] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240405047] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240640404] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240705758] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240734513] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.246172402] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.253283102] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.253322657] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.254378934] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.265071426] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.265334983] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.266198774] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.269233359] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.269286170] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.270174828] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.283118157] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.283457717] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.284038922] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.285851792] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.285892610] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.286579505] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.299158882] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.299602024] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.300175403] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.303461813] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.303501508] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.307538799] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.321246109] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.321600027] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.332091813] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.332152017] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.332474129] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.347624627] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.347992270] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.353576737] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.353617995] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.353902346] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.367456520] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.367776342] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.371653910] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.371689718] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.372203580] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.385205807] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.385518926] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.509216011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 3313] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.761818659] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.761874085] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.763153055] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.779318682] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.780765777] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.780855036] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.786164915] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.788401190] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.791675356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.792764486] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.792809050] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.793171729] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.809111437] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.809467328] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.811063666] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409134.822501084] [controller_manager]: Overrun might occur, Total time : 10357.445 us (Expected < 2000.000 us) --> Read time : 17.353 us, Update time : 10335.984 us (Switch time : 10280.810 us (Switch chained mode time : 0.481 us, perform mode change time : 16.201 us, Activation time : 10247.947 us, Deactivation time : 0.250 us)), Write time : 4.108 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.822524744] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.825671342] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.826861334] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.826896611] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.827403560] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.840998085] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.843664427] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.844058370] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.844151497] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.846907254] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.848256608] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.851081740] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.852289982] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.852328054] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.853566097] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.871777795] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.872079714] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.874172155] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.877245365] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.878373352] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.881642650] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.883008720] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.883041903] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.883441232] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.901294841] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.901712810] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.905153839] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.906336226] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.909352647] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.910869614] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.910908127] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.911244697] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.925152314] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.939485053] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940198287] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940385423] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940402115] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940409920] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.942166191] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.949263162] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.950472304] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.953202953] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 3312] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409136.042665587] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3310] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409136.344135290] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.344194302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.780264713] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.203308 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.806188164] [controller_manager]: Overrun might occur, Total time : 4055.797 us (Expected < 2000.000 us) --> Read time : 55.586 us, Update time : 3998.387 us, Write time : 1.824 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.291528799] [controller_manager]: Overrun might occur, Total time : 3372.678 us (Expected < 2000.000 us) --> Read time : 29.486 us, Update time : 3341.459 us, Write time : 1.733 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.291563585] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.502340 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.497881903] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.820507 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.612694550] [controller_manager]: Overrun might occur, Total time : 4574.598 us (Expected < 2000.000 us) --> Read time : 13.847 us, Update time : 4559.078 us, Write time : 1.673 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409142.320743429] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.682124 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409142.728495692] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409142.728555245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409143.145050259] [controller_manager]: Overrun might occur, Total time : 4905.335 us (Expected < 2000.000 us) --> Read time : 14.808 us, Update time : 4888.733 us, Write time : 1.794 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409143.728655558] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409143.728704812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409144.728891890] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409144.728940272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409145.728664622] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409145.728713004] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409146.728705568] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409146.728752116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409136.222518308] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409147.353295221] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:21 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:52163 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:52163 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.045365582] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.045414214] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.045425586] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.050496542] [moveit_2895436883.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.052393977] [moveit_2895436883.moveit.ros.rdf_loader]: Loaded robot model in 0.00177331 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.052427250] [moveit_2895436883.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.052437039] [moveit_2895436883.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409145.064034325] [moveit_2895436883.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.066741860] [moveit_2895436883.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.124119435] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.124314852] [moveit_2895436883.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125197758] [moveit_2895436883.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125610662] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125625721] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125751275] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126111399] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126196000] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126719174] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126734153] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.127069861] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.127497764] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409145.127776724] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409145.127793025] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.632426495] [moveit_2895436883.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.633152575] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634414122] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634428870] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634775629] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634791399] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634815064] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634824191] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634837777] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.635684866] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.638001489] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.638015806] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.640038239] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.640053417] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.640822338] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.667587055] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.671061547] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.671220008] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.671241970] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.672056778] [moveit_2895436883.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:27] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:27] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409147.484945837] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:42 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:42 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:42 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409162.645452794] [moveit_2895436883.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:13 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_cylinder_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py::test_pick_up_cylinder_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 32.59s ========================= + + +12:33:05.24 [INFO] Long running tasks: + 98.88s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 145.66s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:33:31.06 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests +12:33:31.07 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-2cCn2G +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py::test_side_box[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:33:12 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-47-039672-DESKTOP-HG3Q5KV-4225 (testutils.py:33) + +2026-05-10 12:33:12 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [4236] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [4237] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [4238] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-4]: process started with pid [4239] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-5]: process started with pid [4240] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [robot_state_publisher-2] [INFO] [1778409167.368767141] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.376216632] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.379285082] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.381617003] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.381652019] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.381658692] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409167.381768936] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.548425333] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.551539871] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.551960440] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.552027558] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.560850818] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562042542] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562079272] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562116733] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562128345] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562209108] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562277067] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.669871827] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.922158493] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.922228706] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.924930449] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.937427295] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.938794644] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.938924630] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.947585902] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.948367793] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.951308836] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.952219081] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.952281169] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.958329120] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.971136807] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.971402192] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.972652276] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409167.983430315] [controller_manager]: Overrun might occur, Total time : 9314.058 us (Expected < 2000.000 us) --> Read time : 15.750 us, Update time : 9295.974 us (Switch time : 9244.916 us (Switch chained mode time : 0.361 us, perform mode change time : 14.077 us, Activation time : 9214.398 us, Deactivation time : 0.240 us)), Write time : 2.334 us (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409167.983481693] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.464490 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.983436507] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.984876984] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.985660713] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.985729383] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.986041025] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.995961426] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.997126515] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.997349324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.997391334] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.998885743] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.000216852] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.003112514] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.004042265] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.004087662] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.005100777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.019543896] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.019819950] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.021922681] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.022927605] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.024229413] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.027412227] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.028475371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.028508414] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.028873739] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.041398156] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.041674155] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.044810255] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.046271236] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.049042607] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.050094370] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.050116402] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.050365355] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.060861725] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.072491983] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.072795530] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.072989649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.073016019] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.073039955] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.075862769] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.085343383] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.086469564] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.089348984] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.170792296] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 4239] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409168.406187520] [controller_manager]: Overrun might occur, Total time : 4089.955 us (Expected < 2000.000 us) --> Read time : 13.807 us, Update time : 4074.245 us, Write time : 1.903 us (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.426604695] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.426636415] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.426825575] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.455053647] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456165620] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456325333] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456362764] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456378755] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.457817208] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.465826822] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.465911993] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.467745092] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.481287268] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.481614941] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.482681643] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.485626369] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.485664852] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.486438803] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.507184953] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.510981417] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.511444246] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.518886043] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.518905399] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.519490090] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.549536717] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.549811299] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.551288195] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.557313604] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.557333902] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.557475742] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.580935554] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.581397292] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.594499942] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.595726914] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.596324018] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.614528890] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.615091895] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.623864198] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.623892552] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.624327799] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.639394020] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.639769554] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.644060443] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.644085220] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.644700112] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.662273334] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.662689786] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 4240] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [robot_state_publisher-2] [INFO] [1778409169.826882108] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4237] (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409170.164929478] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409170.164974995] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409172.094218999] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.202437 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409173.210273694] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.256591 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409173.376980454] [controller_manager]: Overrun might occur, Total time : 2885.692 us (Expected < 2000.000 us) --> Read time : 15.519 us, Update time : 2867.789 us, Write time : 2.384 us (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409176.022236848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.220066 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409176.505823894] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409176.505885230] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409177.506166203] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409177.506206189] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409178.506043014] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409178.506098440] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409179.506169548] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409179.506208712] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409180.258470979] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.453686 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409180.506121446] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409180.506174637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409181.277154462] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409181.505995471] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409181.506048071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409181.828258800] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409182.397555090] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409182.397612058] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409182.505980247] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409182.506033788] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409182.876183832] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.166979 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409183.506155943] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409183.506195989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409184.506221121] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409184.506281416] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409185.428444309] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.427427 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409185.506171015] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409185.506229647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409186.506239452] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409186.506283366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409187.506253267] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409187.506294405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409188.506163259] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409188.506201392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409189.506163987] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409189.506218821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409190.506213904] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409190.506259481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409191.506220124] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409191.506286711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409192.506246829] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409192.506286795] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [1778409170.041558519] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] [INFO] [1778409192.774333495] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:33:12 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:32:55 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] * Running on http://127.0.0.1:47889 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] * Running on http://172.30.43.138:47889 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:32:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:32:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.406979936] [moveit_120247871.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.407039289] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.407054087] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.413780372] [moveit_120247871.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.415643377] [moveit_120247871.moveit.ros.rdf_loader]: Loaded robot model in 0.0017229 seconds (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.415670018] [moveit_120247871.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.415679776] [moveit_120247871.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [WARN] [1778409179.427128370] [moveit_120247871.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.430480400] [moveit_120247871.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.485061551] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.485175036] [moveit_120247871.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.485982200] [moveit_120247871.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.486587039] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.486601437] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.486710148] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487062127] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487129395] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487922633] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487937411] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.488419487] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.488928384] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [WARN] [1778409179.489163661] [moveit_120247871.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [ERROR] [1778409179.489176846] [moveit_120247871.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.970445180] [moveit_120247871.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.971198601] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972382832] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972396758] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972746002] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972761151] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972793633] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972805625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972818510] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.973512118] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.976069477] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.976084256] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.978056994] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.978070970] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.978742826] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.007873383] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.011961020] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.012179375] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.012247775] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.013074887] [moveit_120247871.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:01] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:01] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:02 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534388836] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534521899] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534534463] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534551555] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [WARN] [1778409182.534646997] [moveit_120247871.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.535064079] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.535170241] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [ERROR] [1778409192.536104558] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [ERROR] [1778409192.536202013] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:12 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:12] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409192.903586513] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:27 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:27 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:27 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:33:30 [ ERROR] [INFO] [1778409208.044175737] [moveit_120247871.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________________ test_side_box[ur5e] ______________________________ + +start_processes = Urls(ros_domain_id='59', robot_url='http://0.0.0.0:47889', storage_url='http://127.0.0.1:34771') + + @pytest.mark.timeout(400) + def test_side_box(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +> ot.move_to_pose("", start_pose, 0.3, safe=False) + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-47-039672-DESKTOP-HG3Q5KV-4225 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [4236] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [4237] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [4238] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [4239] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [4240] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409167.368767141] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.376216632] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.379285082] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.381617003] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.381652019] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.381658692] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409167.381768936] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.548425333] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.551539871] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.551960440] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.552027558] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.560850818] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562042542] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562079272] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562116733] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562128345] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562209108] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562277067] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.669871827] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.922158493] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.922228706] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.924930449] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.937427295] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.938794644] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.938924630] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.947585902] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.948367793] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.951308836] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.952219081] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.952281169] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.958329120] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.971136807] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.971402192] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.972652276] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409167.983430315] [controller_manager]: Overrun might occur, Total time : 9314.058 us (Expected < 2000.000 us) --> Read time : 15.750 us, Update time : 9295.974 us (Switch time : 9244.916 us (Switch chained mode time : 0.361 us, perform mode change time : 14.077 us, Activation time : 9214.398 us, Deactivation time : 0.240 us)), Write time : 2.334 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409167.983481693] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.464490 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.983436507] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.984876984] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.985660713] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.985729383] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.986041025] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.995961426] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.997126515] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.997349324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.997391334] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.998885743] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.000216852] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.003112514] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.004042265] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.004087662] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.005100777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.019543896] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.019819950] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.021922681] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.022927605] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.024229413] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.027412227] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.028475371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.028508414] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.028873739] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.041398156] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.041674155] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.044810255] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.046271236] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.049042607] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.050094370] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.050116402] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.050365355] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.060861725] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.072491983] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.072795530] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.072989649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.073016019] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.073039955] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.075862769] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.085343383] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.086469564] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.089348984] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.170792296] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 4239] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409168.406187520] [controller_manager]: Overrun might occur, Total time : 4089.955 us (Expected < 2000.000 us) --> Read time : 13.807 us, Update time : 4074.245 us, Write time : 1.903 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.426604695] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.426636415] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.426825575] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.455053647] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456165620] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456325333] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456362764] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456378755] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.457817208] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.465826822] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.465911993] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.467745092] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.481287268] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.481614941] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.482681643] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.485626369] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.485664852] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.486438803] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.507184953] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.510981417] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.511444246] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.518886043] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.518905399] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.519490090] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.549536717] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.549811299] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.551288195] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.557313604] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.557333902] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.557475742] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.580935554] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.581397292] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.594499942] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.595726914] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.596324018] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.614528890] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.615091895] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.623864198] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.623892552] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.624327799] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.639394020] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.639769554] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.644060443] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.644085220] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.644700112] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.662273334] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.662689786] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 4240] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409169.826882108] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4237] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409170.164929478] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409170.164974995] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409172.094218999] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.202437 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409173.210273694] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.256591 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409173.376980454] [controller_manager]: Overrun might occur, Total time : 2885.692 us (Expected < 2000.000 us) --> Read time : 15.519 us, Update time : 2867.789 us, Write time : 2.384 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409176.022236848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.220066 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409176.505823894] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409176.505885230] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409177.506166203] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409177.506206189] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409178.506043014] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409178.506098440] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409179.506169548] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409179.506208712] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409180.258470979] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.453686 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409180.506121446] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409180.506174637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409181.277154462] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409181.505995471] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409181.506048071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409181.828258800] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409182.397555090] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409182.397612058] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409182.505980247] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409182.506033788] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409182.876183832] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.166979 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409183.506155943] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409183.506195989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409184.506221121] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409184.506281416] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409185.428444309] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.427427 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409185.506171015] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409185.506229647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409186.506239452] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409186.506283366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409187.506253267] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409187.506294405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409188.506163259] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409188.506201392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409189.506163987] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409189.506218821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409190.506213904] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409190.506259481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409191.506220124] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409191.506286711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409192.506246829] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409192.506286795] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409170.041558519] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409192.774333495] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:55 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:47889 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:47889 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.406979936] [moveit_120247871.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.407039289] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.407054087] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.413780372] [moveit_120247871.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.415643377] [moveit_120247871.moveit.ros.rdf_loader]: Loaded robot model in 0.0017229 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.415670018] [moveit_120247871.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.415679776] [moveit_120247871.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409179.427128370] [moveit_120247871.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.430480400] [moveit_120247871.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.485061551] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.485175036] [moveit_120247871.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.485982200] [moveit_120247871.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.486587039] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.486601437] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.486710148] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487062127] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487129395] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487922633] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487937411] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.488419487] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.488928384] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409179.489163661] [moveit_120247871.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409179.489176846] [moveit_120247871.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.970445180] [moveit_120247871.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.971198601] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972382832] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972396758] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972746002] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972761151] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972793633] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972805625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972818510] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.973512118] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.976069477] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.976084256] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.978056994] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.978070970] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.978742826] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.007873383] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.011961020] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.012179375] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.012247775] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.013074887] [moveit_120247871.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:01] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:01] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:02 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534388836] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534521899] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534534463] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534551555] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409182.534646997] [moveit_120247871.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.535064079] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.535170241] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409192.536104558] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409192.536202013] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:12 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:12] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409192.903586513] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:27 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:27 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:27 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409208.044175737] [moveit_120247871.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:12 + src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_box.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py::test_side_box[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed +======================== 1 failed, 1 warning in 44.25s ========================= + + +12:33:35.28 [INFO] Long running tasks: + 128.92s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 175.70s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:34:05.31 [INFO] Long running tasks: + 87.11s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests + 158.95s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 205.73s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:34:06.45 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:34:21.24 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests +12:34:21.24 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-Ls8zBX +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py::test_side_sphere[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:34:02 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-33-32-171737-DESKTOP-HG3Q5KV-4936 (testutils.py:33) + +2026-05-10 12:34:02 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [4940] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [4941] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [4942] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-4]: process started with pid [4943] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-5]: process started with pid [4944] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [robot_state_publisher-2] [INFO] [1778409212.465490250] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.470039710] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.472588643] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.475083263] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.475118500] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.475125573] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409212.475233573] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.644206858] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.647442376] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.647898583] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.647982723] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.655995598] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657068182] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657101195] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657121193] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657126433] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657204582] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657246271] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409212.748799587] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.000908490] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.000969957] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.003960965] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.024654190] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025597237] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025827645] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025872971] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025903258] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.031551666] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.038643156] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.038686498] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.039817607] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.052856312] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.053112042] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.053831709] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.056540497] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.056571686] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.057088348] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.068549210] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.068784858] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.069281642] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.070844786] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.070879112] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.071388380] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.082585517] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.082846777] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.083368394] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.084638737] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.084673904] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.088308640] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.100682973] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.100902796] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.108838640] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.108874939] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.109125855] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.122856155] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.123115087] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.129043728] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.129080358] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.129391860] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.142519783] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.142785533] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.145271482] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.145313141] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.145864554] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.158483411] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.158746200] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.262645702] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 4944] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.515020245] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.515069989] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.516412395] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.530817962] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.531750329] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.531822015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.536883723] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.537767072] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.540845588] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.541791454] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.541827021] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.542075473] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.556792297] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.557082083] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.558268578] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409213.567925492] [controller_manager]: Overrun might occur, Total time : 8375.094 us (Expected < 2000.000 us) --> Read time : 11.742 us, Update time : 8360.446 us (Switch time : 8318.877 us (Switch chained mode time : 0.411 us, perform mode change time : 9.408 us, Activation time : 8292.236 us, Deactivation time : 0.080 us)), Write time : 2.906 us (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409213.567984163] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.520516 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.567941738] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.570662618] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.571616601] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.571652499] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.572006171] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.584175594] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.586475765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.586795732] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.586856778] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.588459444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.589661608] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.592624648] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.593737647] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.593775339] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.595098248] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.610849323] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.611177986] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.613209107] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.614399258] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.615678578] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.618651166] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.619559879] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.619596899] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.620000450] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.634782338] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.635070140] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.638576938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.639673808] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.642587343] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.643750663] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.643784217] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.644153509] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.657719676] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.672795973] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673314999] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673428946] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673439566] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673446229] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.674682878] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.680552903] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.681781527] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.684818592] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 4943] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [robot_state_publisher-2] [INFO] [1778409214.967256777] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4941] (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409215.274382448] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409215.274432483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409217.191616263] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.153027 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409218.352173879] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.710332 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409221.182230906] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.767630 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409221.697781825] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409221.697829516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409222.697936231] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409222.697984483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409223.697890043] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409223.697941581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409224.697968400] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409224.698019137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409225.697992066] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409225.698064955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409226.697962898] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409226.698013093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409227.698016159] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409227.698070562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409228.697939558] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409228.697991076] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409229.698095101] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409229.698159694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409230.698014051] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409230.698065038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409230.931120946] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409231.482153959] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409231.697915581] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409231.697966989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409232.165232870] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409232.165296211] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409232.698032353] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409232.698086125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409233.698049344] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409233.698107665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409234.697999703] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409234.698052774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409235.698035235] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409235.698087935] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409236.698088704] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409236.698158177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409237.698092516] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409237.698172237] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409238.698042539] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409238.698096932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409239.698009296] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409239.698064781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409240.698182808] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409240.698258031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409241.698211266] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409241.698270107] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409242.698327532] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409242.698385001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [1778409215.149691446] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] [INFO] [1778409242.791451833] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:34:02 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] * Running on http://127.0.0.1:58023 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] * Running on http://172.30.43.138:58023 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.935718815] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.935784039] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.935801002] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.942716633] [moveit_1973848282.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.944465907] [moveit_1973848282.moveit.ros.rdf_loader]: Loaded robot model in 0.00161206 seconds (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.944497417] [moveit_1973848282.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.944506444] [moveit_1973848282.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [WARN] [1778409223.955738676] [moveit_1973848282.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.958128406] [moveit_1973848282.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.012832016] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.012944970] [moveit_1973848282.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.013815695] [moveit_1973848282.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014312959] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014331895] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014457929] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014809006] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014882216] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.015588619] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.015603848] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.016101844] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.016615060] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [WARN] [1778409224.016848834] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [ERROR] [1778409224.016859464] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.229328013] [moveit_1973848282.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.230404809] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.231901013] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.231917253] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232256628] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232271867] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232295302] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232299911] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232315059] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.233124067] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.235685598] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.235700096] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.237529722] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.237542796] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.238212270] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.267385507] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.271213861] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.271365479] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.271387611] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.272191489] [moveit_1973848282.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:50] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:50] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:52 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441043840] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441166813] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441177443] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441194155] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [WARN] [1778409232.441294165] [moveit_1973848282.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441762796] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441862155] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [ERROR] [1778409242.442338580] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [ERROR] [1778409242.442452246] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:02] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409242.922416753] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:17 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:17 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:17 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:34:20 [ ERROR] [INFO] [1778409258.043515185] [moveit_1973848282.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +____________________________ test_side_sphere[ur5e] ____________________________ + +start_processes = Urls(ros_domain_id='49', robot_url='http://0.0.0.0:58023', storage_url='http://127.0.0.1:40857') + + @pytest.mark.timeout(400) + def test_side_sphere(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +> ot.move_to_pose("", start_pose, 0.3, safe=False) + +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-33-32-171737-DESKTOP-HG3Q5KV-4936 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [4940] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [4941] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [4942] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [4943] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [4944] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409212.465490250] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.470039710] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.472588643] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.475083263] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.475118500] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.475125573] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409212.475233573] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.644206858] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.647442376] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.647898583] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.647982723] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.655995598] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657068182] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657101195] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657121193] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657126433] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657204582] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657246271] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409212.748799587] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.000908490] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.000969957] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.003960965] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.024654190] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025597237] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025827645] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025872971] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025903258] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.031551666] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.038643156] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.038686498] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.039817607] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.052856312] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.053112042] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.053831709] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.056540497] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.056571686] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.057088348] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.068549210] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.068784858] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.069281642] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.070844786] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.070879112] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.071388380] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.082585517] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.082846777] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.083368394] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.084638737] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.084673904] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.088308640] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.100682973] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.100902796] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.108838640] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.108874939] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.109125855] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.122856155] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.123115087] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.129043728] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.129080358] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.129391860] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.142519783] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.142785533] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.145271482] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.145313141] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.145864554] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.158483411] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.158746200] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.262645702] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 4944] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.515020245] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.515069989] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.516412395] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.530817962] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.531750329] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.531822015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.536883723] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.537767072] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.540845588] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.541791454] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.541827021] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.542075473] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.556792297] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.557082083] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.558268578] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409213.567925492] [controller_manager]: Overrun might occur, Total time : 8375.094 us (Expected < 2000.000 us) --> Read time : 11.742 us, Update time : 8360.446 us (Switch time : 8318.877 us (Switch chained mode time : 0.411 us, perform mode change time : 9.408 us, Activation time : 8292.236 us, Deactivation time : 0.080 us)), Write time : 2.906 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409213.567984163] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.520516 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.567941738] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.570662618] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.571616601] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.571652499] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.572006171] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.584175594] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.586475765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.586795732] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.586856778] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.588459444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.589661608] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.592624648] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.593737647] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.593775339] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.595098248] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.610849323] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.611177986] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.613209107] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.614399258] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.615678578] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.618651166] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.619559879] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.619596899] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.620000450] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.634782338] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.635070140] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.638576938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.639673808] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.642587343] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.643750663] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.643784217] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.644153509] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.657719676] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.672795973] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673314999] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673428946] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673439566] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673446229] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.674682878] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.680552903] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.681781527] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.684818592] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 4943] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409214.967256777] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4941] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409215.274382448] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409215.274432483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409217.191616263] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.153027 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409218.352173879] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.710332 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409221.182230906] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.767630 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409221.697781825] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409221.697829516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409222.697936231] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409222.697984483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409223.697890043] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409223.697941581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409224.697968400] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409224.698019137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409225.697992066] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409225.698064955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409226.697962898] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409226.698013093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409227.698016159] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409227.698070562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409228.697939558] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409228.697991076] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409229.698095101] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409229.698159694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409230.698014051] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409230.698065038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409230.931120946] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409231.482153959] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409231.697915581] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409231.697966989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409232.165232870] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409232.165296211] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409232.698032353] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409232.698086125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409233.698049344] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409233.698107665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409234.697999703] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409234.698052774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409235.698035235] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409235.698087935] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409236.698088704] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409236.698158177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409237.698092516] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409237.698172237] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409238.698042539] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409238.698096932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409239.698009296] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409239.698064781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409240.698182808] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409240.698258031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409241.698211266] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409241.698270107] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409242.698327532] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409242.698385001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409215.149691446] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409242.791451833] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:58023 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:58023 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.935718815] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.935784039] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.935801002] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.942716633] [moveit_1973848282.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.944465907] [moveit_1973848282.moveit.ros.rdf_loader]: Loaded robot model in 0.00161206 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.944497417] [moveit_1973848282.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.944506444] [moveit_1973848282.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409223.955738676] [moveit_1973848282.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.958128406] [moveit_1973848282.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.012832016] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.012944970] [moveit_1973848282.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.013815695] [moveit_1973848282.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014312959] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014331895] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014457929] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014809006] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014882216] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.015588619] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.015603848] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.016101844] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.016615060] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409224.016848834] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409224.016859464] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.229328013] [moveit_1973848282.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.230404809] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.231901013] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.231917253] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232256628] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232271867] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232295302] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232299911] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232315059] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.233124067] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.235685598] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.235700096] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.237529722] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.237542796] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.238212270] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.267385507] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.271213861] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.271365479] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.271387611] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.272191489] [moveit_1973848282.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:50] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:50] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:52 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441043840] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441166813] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441177443] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441194155] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409232.441294165] [moveit_1973848282.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441762796] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441862155] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409242.442338580] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409242.442452246] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:02] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409242.922416753] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:17 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:17 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:17 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409258.043515185] [moveit_1973848282.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:12 + src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_sphere.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py::test_side_sphere[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed +======================== 1 failed, 1 warning in 49.27s ========================= + + +12:34:30.63 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:34:35.35 [INFO] Long running tasks: + 117.15s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:34:40.11 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests +12:34:40.12 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-0WZ8YU +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py::test_pick_up_sphere_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:34:21 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-07-459230-DESKTOP-HG3Q5KV-5452 (testutils.py:33) + +2026-05-10 12:34:21 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [5456] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [5457] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [5458] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-4]: process started with pid [5459] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-5]: process started with pid [5460] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778409247.718179587] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.722477102] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.725155402] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.727550032] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.727580860] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.727586371] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409247.727680149] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.735680410] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.738127049] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.738569500] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.738644001] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.746282490] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747347785] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747375728] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747399924] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747404332] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747452023] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747484094] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409247.984163459] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.236373294] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.236437406] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.240143818] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.263055593] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264072475] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264284238] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264327360] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264348590] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.268535075] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.275118263] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.275160533] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.276230046] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.286949794] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.287175959] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.287935211] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.289141238] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.289176044] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.289703035] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.302839507] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.303071247] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.303560427] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.305130099] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.305165827] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.305819238] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.319249907] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.319473031] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.319961540] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.321377240] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.321418328] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.325251522] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.337077302] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.337318971] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.345490574] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.345533416] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.345798389] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.361151860] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.361392603] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.367441107] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.367470803] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.367723924] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.381147631] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.381475423] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.385845887] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.385884811] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.386579671] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.399239210] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.399572830] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.498576222] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 5460] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.751130986] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.751185249] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.752508879] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.767162819] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.768239129] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.768306688] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.773571887] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.774377999] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.776988368] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.777876636] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.777912484] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.778299844] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.790909834] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.791180213] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.792680100] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409248.802300875] [controller_manager]: Overrun might occur, Total time : 8247.351 us (Expected < 2000.000 us) --> Read time : 12.794 us, Update time : 8231.912 us (Switch time : 8186.696 us (Switch chained mode time : 0.260 us, perform mode change time : 9.778 us, Activation time : 8161.538 us, Deactivation time : 0.070 us)), Write time : 2.645 us (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409248.802355418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.375299 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.802318158] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.805132505] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.806146065] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.806189057] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.806537299] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.818062554] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.819097761] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.819360129] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.819414292] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.822961893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.824198057] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.827157479] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.828392261] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.828427878] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.829453728] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.847062436] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.847281442] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.849129438] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.850740490] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.852161780] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.855007397] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.855990320] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.856029715] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.856298861] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.873067663] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.873402555] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.876886920] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.878233995] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.881185614] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.882362635] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.882398072] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.882685438] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.897234809] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.911151622] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911469020] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911664351] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911710598] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911723834] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.913298616] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.919321717] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.920338297] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.923605826] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 5459] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778409250.274436007] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5457] (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409250.552126363] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409250.552181568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409252.481055497] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.075528 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409253.627411756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.431847 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409256.594211181] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.231432 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409256.897872969] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409256.897918215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409257.200541906] [controller_manager]: Overrun might occur, Total time : 2499.329 us (Expected < 2000.000 us) --> Read time : 13.536 us, Update time : 2484.210 us, Write time : 1.583 us (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409257.898023018] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409257.898066320] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409258.898193064] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409258.898238501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409259.898108052] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409259.898203262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409260.898187361] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409260.898220494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409261.273475272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.495503 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [1778409250.440024799] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] [INFO] [1778409261.373104140] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:34:21 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] * Running on http://127.0.0.1:46689 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] * Running on http://172.30.43.138:46689 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:19 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.099363680] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.099415108] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.099431409] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.103312629] [moveit_3975733361.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.105138488] [moveit_3975733361.moveit.ros.rdf_loader]: Loaded robot model in 0.00169144 seconds (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.105167072] [moveit_3975733361.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.105175428] [moveit_3975733361.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [WARN] [1778409260.117255207] [moveit_3975733361.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.120208057] [moveit_3975733361.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.174133451] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.174351330] [moveit_3975733361.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175204942] [moveit_3975733361.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175544628] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175558344] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175657622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175879644] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175956901] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.176519622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.176534690] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.177092480] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.177416159] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [WARN] [1778409260.177628613] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [ERROR] [1778409260.177643361] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.450338013] [moveit_3975733361.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.451324102] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.452727889] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.452742968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453070911] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453086109] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453116577] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453126827] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453140593] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.454045572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.456394001] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.456410182] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.458842353] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.458858554] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.459469004] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.487247385] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.489924833] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.490087843] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.490117600] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.490854400] [moveit_3975733361.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:21] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:21] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409261.534614756] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:36 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:36 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:36 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:34:39 [ ERROR] [INFO] [1778409276.674727546] [moveit_3975733361.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_______________________ test_pick_up_sphere_by_id[ur5e] ________________________ + +start_processes = Urls(ros_domain_id='132', robot_url='http://0.0.0.0:46689', storage_url='http://127.0.0.1:59497') + + @pytest.mark.timeout(321) + def test_pick_up_sphere_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Sphere("Sphere1", 0.1) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Sphere1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-07-459230-DESKTOP-HG3Q5KV-5452 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [5456] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [5457] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [5458] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [5459] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [5460] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409247.718179587] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.722477102] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.725155402] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.727550032] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.727580860] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.727586371] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409247.727680149] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.735680410] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.738127049] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.738569500] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.738644001] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.746282490] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747347785] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747375728] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747399924] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747404332] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747452023] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747484094] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409247.984163459] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.236373294] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.236437406] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.240143818] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.263055593] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264072475] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264284238] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264327360] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264348590] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.268535075] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.275118263] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.275160533] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.276230046] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.286949794] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.287175959] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.287935211] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.289141238] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.289176044] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.289703035] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.302839507] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.303071247] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.303560427] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.305130099] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.305165827] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.305819238] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.319249907] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.319473031] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.319961540] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.321377240] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.321418328] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.325251522] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.337077302] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.337318971] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.345490574] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.345533416] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.345798389] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.361151860] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.361392603] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.367441107] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.367470803] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.367723924] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.381147631] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.381475423] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.385845887] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.385884811] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.386579671] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.399239210] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.399572830] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.498576222] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 5460] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.751130986] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.751185249] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.752508879] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.767162819] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.768239129] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.768306688] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.773571887] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.774377999] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.776988368] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.777876636] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.777912484] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.778299844] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.790909834] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.791180213] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.792680100] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409248.802300875] [controller_manager]: Overrun might occur, Total time : 8247.351 us (Expected < 2000.000 us) --> Read time : 12.794 us, Update time : 8231.912 us (Switch time : 8186.696 us (Switch chained mode time : 0.260 us, perform mode change time : 9.778 us, Activation time : 8161.538 us, Deactivation time : 0.070 us)), Write time : 2.645 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409248.802355418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.375299 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.802318158] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.805132505] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.806146065] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.806189057] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.806537299] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.818062554] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.819097761] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.819360129] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.819414292] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.822961893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.824198057] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.827157479] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.828392261] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.828427878] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.829453728] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.847062436] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.847281442] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.849129438] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.850740490] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.852161780] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.855007397] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.855990320] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.856029715] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.856298861] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.873067663] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.873402555] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.876886920] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.878233995] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.881185614] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.882362635] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.882398072] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.882685438] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.897234809] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.911151622] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911469020] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911664351] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911710598] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911723834] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.913298616] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.919321717] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.920338297] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.923605826] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 5459] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409250.274436007] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5457] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409250.552126363] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409250.552181568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409252.481055497] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.075528 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409253.627411756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.431847 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409256.594211181] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.231432 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409256.897872969] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409256.897918215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409257.200541906] [controller_manager]: Overrun might occur, Total time : 2499.329 us (Expected < 2000.000 us) --> Read time : 13.536 us, Update time : 2484.210 us, Write time : 1.583 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409257.898023018] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409257.898066320] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409258.898193064] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409258.898238501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409259.898108052] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409259.898203262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409260.898187361] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409260.898220494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409261.273475272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.495503 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409250.440024799] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409261.373104140] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:46689 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:46689 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:19 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.099363680] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.099415108] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.099431409] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.103312629] [moveit_3975733361.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.105138488] [moveit_3975733361.moveit.ros.rdf_loader]: Loaded robot model in 0.00169144 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.105167072] [moveit_3975733361.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.105175428] [moveit_3975733361.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409260.117255207] [moveit_3975733361.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.120208057] [moveit_3975733361.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.174133451] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.174351330] [moveit_3975733361.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175204942] [moveit_3975733361.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175544628] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175558344] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175657622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175879644] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175956901] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.176519622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.176534690] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.177092480] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.177416159] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409260.177628613] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409260.177643361] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.450338013] [moveit_3975733361.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.451324102] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.452727889] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.452742968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453070911] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453086109] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453116577] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453126827] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453140593] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.454045572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.456394001] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.456410182] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.458842353] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.458858554] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.459469004] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.487247385] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.489924833] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.490087843] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.490117600] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.490854400] [moveit_3975733361.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:21] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:21] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409261.534614756] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409276.674727546] [moveit_3975733361.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:13 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_sphere_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py::test_pick_up_sphere_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 32.66s ========================= + + +12:35:05.43 [INFO] Long running tasks: + 147.23s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:35:24.52 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests +12:35:24.52 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-qZ6cxi +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py::test_graspable[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:35:05 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-41-439940-DESKTOP-HG3Q5KV-6668 (testutils.py:33) + +2026-05-10 12:35:05 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [6698] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [6699] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [6700] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-4]: process started with pid [6701] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-5]: process started with pid [6702] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.788077567] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778409281.790567232] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.791061877] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.793577958] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.793596723] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.793602304] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.793840210] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.801718947] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.803914924] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.804282773] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.804388795] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.811330154] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812545584] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812576292] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812596070] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812601430] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812655683] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812696020] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.106808994] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.359004917] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.359071063] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.362008891] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.375328284] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.376171476] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.376308717] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.383658091] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.384475163] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.387522603] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.388368942] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.388400271] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.392839857] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.405484560] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.405788045] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.406771840] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.418864822] [controller_manager]: Overrun might occur, Total time : 10708.702 us (Expected < 2000.000 us) --> Read time : 17.022 us, Update time : 10687.602 us (Switch time : 10659.379 us (Switch chained mode time : 0.301 us, perform mode change time : 15.189 us, Activation time : 10641.204 us, Deactivation time : 0.330 us)), Write time : 4.078 us (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.418904347] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.824074 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.418931489] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.421680683] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.422686204] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.422759052] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.423179902] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.432910010] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.435649345] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.436172419] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.436221412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.437850133] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.438255439] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.441295459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.442448279] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.442470311] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.443601631] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.457416671] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.457624290] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.460011256] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.462904895] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.464276230] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.467412930] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.468604931] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.468635659] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.468886576] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.481635059] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.481917581] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.485045504] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.486313995] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.489154231] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.490247568] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.490270352] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.490582260] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.502172287] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.513709168] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514776758] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514933927] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514959475] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514979664] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.517342523] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.523326522] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.524553986] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.527628146] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.627385753] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 6701] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.879925905] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.880009664] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.880302600] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.901516269] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903344929] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903536213] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903558204] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903567642] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.904886202] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.911907710] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.911965489] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.913369391] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.927611302] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.927946238] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.928755316] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.931599731] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.931636631] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.932432699] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.947418829] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.948195314] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.948789934] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.951893316] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.951917111] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.952762142] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.965632577] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.965955641] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.966475368] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.970015070] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.970037873] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.970381772] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.983224674] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.983524985] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.993827066] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.993919632] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.994168906] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409283.009736646] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.010110711] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.015439542] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.015480069] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.015774759] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409283.031354903] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.031675607] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.036049323] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.036073238] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.036402915] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409283.055820295] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.056323001] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 6702] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409283.882227362] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.146327 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778409284.188563654] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6699] (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409284.542105761] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409284.542161126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.136269142] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.188288 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.318366848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.286294 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.372122650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.042577 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.410296108] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.215584 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.754266594] [controller_manager]: Overrun might occur, Total time : 2121.971 us (Expected < 2000.000 us) --> Read time : 15.970 us, Update time : 2104.088 us, Write time : 1.913 us (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.450336154] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.255950 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409291.014211003] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.014276738] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.460968420] [controller_manager]: Overrun might occur, Total time : 4794.045 us (Expected < 2000.000 us) --> Read time : 34.947 us, Update time : 4756.573 us, Write time : 2.525 us (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.461015790] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.935306 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409292.014011352] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.014060736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409293.014057340] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.014095903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.886275650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.195266 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.014882028] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.014953859] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.338521827] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.889535425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.013973206] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.014030104] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.459765375] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.459807775] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409296.014038170] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.014091491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.454290217] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.209833 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409297.014083513] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409297.014162393] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409298.014047494] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.014095826] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.199527048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.446714 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409299.013913916] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409299.013963600] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409300.013975980] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.014041103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.269611608] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.530994 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409301.014053707] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409301.014109062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409302.014248366] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409302.014302429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409303.014067599] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409303.014145978] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409304.013980739] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409304.014069378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409305.014351460] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.014405883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.350305056] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.224703 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [1778409284.433083616] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:35:05 [ ERROR] [INFO] [1778409305.873138648] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:06 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:35:06 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:50 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] * Running on http://127.0.0.1:50169 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] * Running on http://172.30.43.138:50169 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.731355588] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.731395063] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.731405563] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.738733317] [moveit_1824579522.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.740921594] [moveit_1824579522.moveit.ros.rdf_loader]: Loaded robot model in 0.0020249 seconds (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.740952092] [moveit_1824579522.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.740959727] [moveit_1824579522.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [WARN] [1778409293.755298157] [moveit_1824579522.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.758242526] [moveit_1824579522.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.821791196] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.821915973] [moveit_1824579522.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.822937144] [moveit_1824579522.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.823504803] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.823519110] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.823649277] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824136223] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824198872] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824719180] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824731123] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.825424696] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.825984981] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [WARN] [1778409293.826311371] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [ERROR] [1778409293.826332812] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.877052195] [moveit_1824579522.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.877965430] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879417850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879435122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879733188] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879746494] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879799694] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879815625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879833940] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.880716226] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.883475879] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.883493232] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.886432112] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.886447632] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.887341986] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.918199460] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.922162563] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.922366501] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.922404583] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.923247615] [moveit_1824579522.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:54] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:54] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:55 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471068818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471179549] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471189337] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471205438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [WARN] [1778409295.471300959] [moveit_1824579522.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471711048] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471842789] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [ERROR] [1778409305.472574677] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [ERROR] [1778409305.472682733] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:05 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:05] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409306.002848988] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:21 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:21 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:21 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:35:24 [ ERROR] [INFO] [1778409321.171936987] [moveit_1824579522.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________________ test_graspable[ur5e] _____________________________ + +start_processes = Urls(ros_domain_id='101', robot_url='http://0.0.0.0:50169', storage_url='http://127.0.0.1:50735') + + @pytest.mark.timeout(400) + def test_graspable(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.5 + Y = 0.0 + Z = 0.3 +  + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) +> ot.move_to_pose("", start_pose, 0.3, safe=False) + +src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:28: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose + self.move( +src/python/arcor2_ur/object_types/ur5e.py:149: in move + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-41-439940-DESKTOP-HG3Q5KV-6668 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [6698] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [6699] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [6700] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [6701] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [6702] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.788077567] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409281.790567232] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.791061877] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.793577958] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.793596723] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.793602304] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.793840210] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.801718947] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.803914924] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.804282773] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.804388795] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.811330154] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812545584] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812576292] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812596070] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812601430] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812655683] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812696020] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.106808994] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.359004917] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.359071063] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.362008891] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.375328284] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.376171476] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.376308717] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.383658091] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.384475163] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.387522603] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.388368942] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.388400271] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.392839857] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.405484560] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.405788045] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.406771840] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.418864822] [controller_manager]: Overrun might occur, Total time : 10708.702 us (Expected < 2000.000 us) --> Read time : 17.022 us, Update time : 10687.602 us (Switch time : 10659.379 us (Switch chained mode time : 0.301 us, perform mode change time : 15.189 us, Activation time : 10641.204 us, Deactivation time : 0.330 us)), Write time : 4.078 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.418904347] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.824074 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.418931489] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.421680683] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.422686204] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.422759052] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.423179902] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.432910010] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.435649345] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.436172419] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.436221412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.437850133] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.438255439] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.441295459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.442448279] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.442470311] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.443601631] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.457416671] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.457624290] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.460011256] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.462904895] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.464276230] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.467412930] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.468604931] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.468635659] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.468886576] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.481635059] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.481917581] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.485045504] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.486313995] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.489154231] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.490247568] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.490270352] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.490582260] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.502172287] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.513709168] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514776758] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514933927] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514959475] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514979664] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.517342523] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.523326522] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.524553986] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.527628146] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.627385753] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 6701] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.879925905] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.880009664] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.880302600] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.901516269] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903344929] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903536213] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903558204] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903567642] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.904886202] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.911907710] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.911965489] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.913369391] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.927611302] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.927946238] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.928755316] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.931599731] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.931636631] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.932432699] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.947418829] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.948195314] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.948789934] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.951893316] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.951917111] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.952762142] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.965632577] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.965955641] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.966475368] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.970015070] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.970037873] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.970381772] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.983224674] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.983524985] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.993827066] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.993919632] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.994168906] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409283.009736646] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.010110711] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.015439542] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.015480069] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.015774759] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409283.031354903] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.031675607] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.036049323] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.036073238] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.036402915] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409283.055820295] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.056323001] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 6702] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409283.882227362] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.146327 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409284.188563654] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6699] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409284.542105761] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409284.542161126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.136269142] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.188288 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.318366848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.286294 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.372122650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.042577 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.410296108] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.215584 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.754266594] [controller_manager]: Overrun might occur, Total time : 2121.971 us (Expected < 2000.000 us) --> Read time : 15.970 us, Update time : 2104.088 us, Write time : 1.913 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.450336154] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.255950 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409291.014211003] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.014276738] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.460968420] [controller_manager]: Overrun might occur, Total time : 4794.045 us (Expected < 2000.000 us) --> Read time : 34.947 us, Update time : 4756.573 us, Write time : 2.525 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.461015790] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.935306 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409292.014011352] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.014060736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409293.014057340] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.014095903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.886275650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.195266 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.014882028] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.014953859] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.338521827] [io_and_status_controller]: Setting speed slider to 30.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.889535425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.013973206] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.014030104] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.459765375] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.459807775] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409296.014038170] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.014091491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.454290217] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.209833 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409297.014083513] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409297.014162393] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409298.014047494] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.014095826] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.199527048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.446714 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409299.013913916] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409299.013963600] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409300.013975980] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.014041103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.269611608] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.530994 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409301.014053707] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409301.014109062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409302.014248366] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409302.014302429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409303.014067599] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409303.014145978] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409304.013980739] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409304.014069378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409305.014351460] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.014405883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.350305056] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.224703 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409284.433083616] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409305.873138648] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:50169 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:50169 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.731355588] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.731395063] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.731405563] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.738733317] [moveit_1824579522.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.740921594] [moveit_1824579522.moveit.ros.rdf_loader]: Loaded robot model in 0.0020249 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.740952092] [moveit_1824579522.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.740959727] [moveit_1824579522.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409293.755298157] [moveit_1824579522.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.758242526] [moveit_1824579522.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.821791196] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.821915973] [moveit_1824579522.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.822937144] [moveit_1824579522.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.823504803] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.823519110] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.823649277] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824136223] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824198872] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824719180] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824731123] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.825424696] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.825984981] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409293.826311371] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409293.826332812] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.877052195] [moveit_1824579522.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.877965430] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879417850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879435122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879733188] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879746494] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879799694] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879815625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879833940] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.880716226] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.883475879] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.883493232] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.886432112] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.886447632] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.887341986] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.918199460] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.922162563] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.922366501] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.922404583] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.923247615] [moveit_1824579522.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:54] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:54] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471068818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471179549] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471189337] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471205438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409295.471300959] [moveit_1824579522.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471711048] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471842789] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409305.472574677] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409305.472682733] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:05] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409306.002848988] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.171936987] [moveit_1824579522.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:13 + src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_collision.test_obstacle_graspable.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py::test_graspable[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed +======================== 1 failed, 1 warning in 43.36s ========================= + + +12:35:35.47 [INFO] Long running tasks: + 64.84s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests + 74.24s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests + 177.27s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:36:01.41 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests +12:36:01.42 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-cU2fDa +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py::test_pick_and_place_box_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:35:43 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-22-272481-DESKTOP-HG3Q5KV-5908 (testutils.py:33) + +2026-05-10 12:35:43 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [5912] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [5913] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [5914] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-4]: process started with pid [5915] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-5]: process started with pid [5916] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [robot_state_publisher-2] [INFO] [1778409262.549551434] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.550457761] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.553354602] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.555612692] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.555644413] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.555651045] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409262.555761630] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.729985698] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.732621687] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.732998733] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.733066853] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.741374118] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742687063] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742721548] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742745754] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742751355] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742805768] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742844001] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409262.803210390] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.055317185] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.055374063] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.058681608] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.080953021] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.081839772] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.081992482] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.082034442] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.082059630] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.086465710] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.093388224] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.093430134] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.094539742] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.104989910] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.105207317] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.105966290] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.107140651] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.107177591] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.107740130] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.119096047] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.119339585] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.119954905] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.121406112] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.121440568] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.121960365] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.133002356] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.133243524] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.133744436] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.134985885] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.135016714] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.138622150] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.151262243] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.151503941] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.159180969] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.159218540] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.159600185] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.171009680] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.171267054] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.177676538] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.177711404] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.178019029] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.191023910] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.191270394] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.193976871] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.194014963] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.194618209] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.206927329] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.207167320] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.308475819] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 5916] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.560625400] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.560675385] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.561966084] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.575637796] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.576889119] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.576951507] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.581845691] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.582339024] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.585310080] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.586413045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.586448994] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.586850907] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.600894532] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.601178251] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.602805127] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409263.612952985] [controller_manager]: Overrun might occur, Total time : 8898.944 us (Expected < 2000.000 us) --> Read time : 11.702 us, Update time : 8884.737 us (Switch time : 8840.524 us (Switch chained mode time : 0.441 us, perform mode change time : 9.799 us, Activation time : 8813.131 us, Deactivation time : 0.140 us)), Write time : 2.505 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409263.613015333] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.037376 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.612969140] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.615170858] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.616300826] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.616341373] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.616779290] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.628921080] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.631006789] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.631341751] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.631401394] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.634990744] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.636187353] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.639094170] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.640163719] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.640199907] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.641309345] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.657238744] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.657506522] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.659464733] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.660829135] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.662209137] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.665137747] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.666263536] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.666306458] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.666634035] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.680954336] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.681254321] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.684966910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.686215437] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.689020031] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.690004677] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.690041317] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.690488281] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.703309739] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.717557506] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718113307] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718266868] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718282738] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718289942] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.719909455] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.726856455] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.728363854] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.731018569] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 5915] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [robot_state_publisher-2] [INFO] [1778409265.094024084] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5913] (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409265.387438825] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409265.387482358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409267.302218411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.240925 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409268.521119103] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.141647 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409270.655726791] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.749215 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409271.917770519] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409271.917824671] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409272.238216327] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.238751 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.854995613] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409272.855053884] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.855185279] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409273.855238721] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409274.855103029] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409274.855145400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409275.855039112] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409275.855091342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409276.855195118] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409276.855249081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.018341836] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 12.364320 ms (missed cycles : 7). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.060080729] [controller_manager]: Overrun might occur, Total time : 2015.961 us (Expected < 2000.000 us) --> Read time : 1952.540 us, Update time : 61.668 us, Write time : 1.753 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409277.855301274] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.855699701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409277.892541089] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.225895372] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.917836 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.444744492] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409278.855168922] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.855225119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409279.008537622] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.008582166] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.051281405] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.051343844] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.855302616] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409279.855345318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409280.124092009] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.114303 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409280.316084873] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409280.855014189] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409280.855058113] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.126164413] [controller_manager]: Overrun might occur, Total time : 2128.183 us (Expected < 2000.000 us) --> Read time : 14.557 us, Update time : 2111.993 us, Write time : 1.633 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.126206313] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.228766 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.855296851] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.855377734] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.855067697] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.855105298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.855059245] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409283.855114179] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409284.854982896] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409284.855040505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409285.855018714] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409285.855073298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.139390759] [controller_manager]: Overrun might occur, Total time : 3316.512 us (Expected < 2000.000 us) --> Read time : 3181.095 us, Update time : 133.043 us, Write time : 2.374 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.139435113] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.457296 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409286.855389070] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.855429177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.678614592] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.636926 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409287.855142507] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.855653993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.856549159] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.856596519] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.386304226] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.326620 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409289.855153616] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.855191267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.450298411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.320805 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409290.854903404] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.854956374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.474354175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.376739 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409291.854858784] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.854933897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.765276491] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.298504 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.772776262] [controller_manager]: Overrun might occur, Total time : 2990.028 us (Expected < 2000.000 us) --> Read time : 28.615 us, Update time : 2958.677 us, Write time : 2.736 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409292.854952221] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.855007917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409293.855045529] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.855097668] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.906289378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.311942 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.962254353] [controller_manager]: Overrun might occur, Total time : 2172.272 us (Expected < 2000.000 us) --> Read time : 20.940 us, Update time : 2148.837 us, Write time : 2.495 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.854928226] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.854985916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.854884480] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.854938803] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409296.854941558] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.854985982] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409297.854830332] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409297.854896297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.217584925] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.607228 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409298.854972974] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.855014423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409299.854979937] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409299.855036505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409300.854935218] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.854998027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409301.855057655] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409301.855111458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409302.854942451] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409302.854998348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409303.855037568] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409303.855097532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409304.854979272] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409304.855039496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409305.855825209] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.855870064] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409306.854886888] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409306.854939358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409307.854910257] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409307.854979268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409308.854918743] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409308.854973277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409309.854883406] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409309.854941236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409310.462073398] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409310.854897887] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409310.854945647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.013138704] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.586948576] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409311.587046582] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409311.854893328] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.854946479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.160861944] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.160926577] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.754086783] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.854867122] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409312.854908330] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409313.854914692] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409313.854968284] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409314.854924200] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409314.854977781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409315.855092087] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409315.855140990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409316.855040709] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409316.855096445] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409317.855064449] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409317.855114585] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409318.855057005] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409318.855111879] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409319.855238653] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409319.855305129] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409320.854835531] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409320.854886108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.855049995] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409321.855124316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409322.855096098] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409322.855149469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409323.855141204] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409323.855192321] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.738210221] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.232475 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.804632913] [controller_manager]: Overrun might occur, Total time : 2596.895 us (Expected < 2000.000 us) --> Read time : 13.135 us, Update time : 2582.237 us, Write time : 1.523 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409324.854939402] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.854985470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.855082794] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409325.855139452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.855056084] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409326.855109715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.854848193] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.854900312] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409328.855001595] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409328.855048754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409329.855052657] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409329.855100838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.678223647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.246081 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409330.855031462] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.855082169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.858219359] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.241863 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409331.858556786] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.858575231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.898167602] [controller_manager]: Overrun might occur, Total time : 2125.560 us (Expected < 2000.000 us) --> Read time : 15.640 us, Update time : 2108.277 us, Write time : 1.643 us (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409332.855033873] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409332.855110619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409333.855163547] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409333.855239060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.644221367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.243630 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409334.856865088] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.856899333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409335.855073861] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.855123716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409336.854994002] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409336.855044108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409337.854917940] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409337.854990809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409338.854997609] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409338.855049517] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409339.854978444] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.855031655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409340.854946146] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409340.854991112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409341.855069902] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409341.855133613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.855030043] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.855088283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [1778409265.263090998] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] [INFO] [1778409274.713339023] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:35:43 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:30 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] * Running on http://127.0.0.1:60145 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] * Running on http://172.30.43.138:60145 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.402644087] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.402700184] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.402711756] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.409908716] [moveit_2533370578.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.412109117] [moveit_2533370578.moveit.ros.rdf_loader]: Loaded robot model in 0.00203912 seconds (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.412141308] [moveit_2533370578.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.412151637] [moveit_2533370578.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [WARN] [1778409274.425153708] [moveit_2533370578.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.427943008] [moveit_2533370578.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.482222837] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.482347303] [moveit_2533370578.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483198170] [moveit_2533370578.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483662683] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483678102] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483788482] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484138427] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484197670] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484879561] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484894129] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.485428534] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.485933013] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [WARN] [1778409274.486267238] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [ERROR] [1778409274.486289971] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.996951404] [moveit_2533370578.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.997829577] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999298904] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999314924] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999567334] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999579918] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999601980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999606539] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999617009] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.000486571] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.002854901] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.002868197] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.005024223] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.005039031] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.005709815] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.031011302] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.034492758] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.034671978] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.034697547] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.035510742] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:36] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  Generated 2 grasp options for object Box1. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  IK accepted: 2 grasp options. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019170695] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019279131] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019300111] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019324658] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [WARN] [1778409279.019427403] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019959023] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.020076476] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.046283462] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.046829069] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.047552239] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Executing plan (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048809317] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048833904] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048852910] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048866256] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048898347] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050387992] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050470579] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050505686] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050666902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.051611853] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.051630799] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409280.352492091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409280.358335540] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:55 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139627241] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139710138] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139722472] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139735567] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [WARN] [1778409312.139927632] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.140204899] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.140345817] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.157245027] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.157400303] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.157844907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Executing plan (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159879733] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159899421] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159913828] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159921002] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159937012] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160386456] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160419599] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160441090] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160580445] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.161194666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.161207751] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.761542234] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.766341108] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:27 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:43 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:43] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409343.176302110] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:58 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:58 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:58 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:36:01 [ ERROR] [INFO] [1778409358.313757967] [moveit_2533370578.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +__________________ test_pick_and_place_box_by_position[ur5e] ___________________ + +start_processes = Urls(ros_domain_id='14', robot_url='http://0.0.0.0:60145', storage_url='http://127.0.0.1:43829') + + @pytest.mark.timeout(321) + def test_pick_and_place_box_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Box1. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-22-272481-DESKTOP-HG3Q5KV-5908 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [5912] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [5913] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [5914] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [5915] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [5916] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409262.549551434] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.550457761] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.553354602] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.555612692] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.555644413] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.555651045] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409262.555761630] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.729985698] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.732621687] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.732998733] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.733066853] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.741374118] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742687063] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742721548] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742745754] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742751355] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742805768] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742844001] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409262.803210390] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.055317185] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.055374063] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.058681608] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.080953021] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.081839772] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.081992482] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.082034442] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.082059630] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.086465710] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.093388224] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.093430134] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.094539742] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.104989910] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.105207317] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.105966290] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.107140651] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.107177591] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.107740130] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.119096047] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.119339585] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.119954905] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.121406112] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.121440568] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.121960365] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.133002356] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.133243524] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.133744436] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.134985885] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.135016714] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.138622150] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.151262243] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.151503941] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.159180969] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.159218540] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.159600185] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.171009680] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.171267054] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.177676538] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.177711404] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.178019029] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.191023910] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.191270394] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.193976871] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.194014963] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.194618209] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.206927329] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.207167320] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.308475819] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 5916] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.560625400] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.560675385] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.561966084] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.575637796] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.576889119] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.576951507] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.581845691] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.582339024] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.585310080] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.586413045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.586448994] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.586850907] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.600894532] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.601178251] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.602805127] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409263.612952985] [controller_manager]: Overrun might occur, Total time : 8898.944 us (Expected < 2000.000 us) --> Read time : 11.702 us, Update time : 8884.737 us (Switch time : 8840.524 us (Switch chained mode time : 0.441 us, perform mode change time : 9.799 us, Activation time : 8813.131 us, Deactivation time : 0.140 us)), Write time : 2.505 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409263.613015333] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.037376 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.612969140] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.615170858] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.616300826] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.616341373] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.616779290] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.628921080] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.631006789] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.631341751] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.631401394] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.634990744] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.636187353] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.639094170] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.640163719] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.640199907] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.641309345] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.657238744] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.657506522] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.659464733] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.660829135] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.662209137] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.665137747] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.666263536] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.666306458] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.666634035] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.680954336] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.681254321] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.684966910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.686215437] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.689020031] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.690004677] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.690041317] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.690488281] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.703309739] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.717557506] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718113307] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718266868] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718282738] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718289942] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.719909455] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.726856455] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.728363854] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.731018569] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 5915] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409265.094024084] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5913] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409265.387438825] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409265.387482358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409267.302218411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.240925 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409268.521119103] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.141647 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409270.655726791] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.749215 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409271.917770519] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409271.917824671] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409272.238216327] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.238751 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.854995613] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409272.855053884] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.855185279] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409273.855238721] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409274.855103029] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409274.855145400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409275.855039112] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409275.855091342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409276.855195118] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409276.855249081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.018341836] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 12.364320 ms (missed cycles : 7). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.060080729] [controller_manager]: Overrun might occur, Total time : 2015.961 us (Expected < 2000.000 us) --> Read time : 1952.540 us, Update time : 61.668 us, Write time : 1.753 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409277.855301274] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.855699701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409277.892541089] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.225895372] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.917836 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.444744492] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409278.855168922] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.855225119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409279.008537622] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.008582166] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.051281405] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.051343844] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.855302616] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409279.855345318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409280.124092009] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.114303 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409280.316084873] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409280.855014189] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409280.855058113] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.126164413] [controller_manager]: Overrun might occur, Total time : 2128.183 us (Expected < 2000.000 us) --> Read time : 14.557 us, Update time : 2111.993 us, Write time : 1.633 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.126206313] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.228766 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.855296851] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.855377734] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.855067697] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.855105298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.855059245] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409283.855114179] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409284.854982896] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409284.855040505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409285.855018714] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409285.855073298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.139390759] [controller_manager]: Overrun might occur, Total time : 3316.512 us (Expected < 2000.000 us) --> Read time : 3181.095 us, Update time : 133.043 us, Write time : 2.374 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.139435113] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.457296 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409286.855389070] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.855429177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.678614592] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.636926 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409287.855142507] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.855653993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.856549159] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.856596519] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.386304226] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.326620 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409289.855153616] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.855191267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.450298411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.320805 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409290.854903404] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.854956374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.474354175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.376739 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409291.854858784] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.854933897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.765276491] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.298504 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.772776262] [controller_manager]: Overrun might occur, Total time : 2990.028 us (Expected < 2000.000 us) --> Read time : 28.615 us, Update time : 2958.677 us, Write time : 2.736 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409292.854952221] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.855007917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409293.855045529] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.855097668] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.906289378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.311942 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.962254353] [controller_manager]: Overrun might occur, Total time : 2172.272 us (Expected < 2000.000 us) --> Read time : 20.940 us, Update time : 2148.837 us, Write time : 2.495 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.854928226] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.854985916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.854884480] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.854938803] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409296.854941558] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.854985982] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409297.854830332] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409297.854896297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.217584925] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.607228 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409298.854972974] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.855014423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409299.854979937] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409299.855036505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409300.854935218] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.854998027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409301.855057655] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409301.855111458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409302.854942451] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409302.854998348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409303.855037568] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409303.855097532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409304.854979272] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409304.855039496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409305.855825209] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.855870064] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409306.854886888] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409306.854939358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409307.854910257] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409307.854979268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409308.854918743] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409308.854973277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409309.854883406] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409309.854941236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409310.462073398] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409310.854897887] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409310.854945647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.013138704] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.586948576] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409311.587046582] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409311.854893328] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.854946479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.160861944] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.160926577] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.754086783] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.854867122] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409312.854908330] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409313.854914692] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409313.854968284] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409314.854924200] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409314.854977781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409315.855092087] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409315.855140990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409316.855040709] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409316.855096445] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409317.855064449] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409317.855114585] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409318.855057005] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409318.855111879] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409319.855238653] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409319.855305129] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409320.854835531] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409320.854886108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.855049995] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409321.855124316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409322.855096098] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409322.855149469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409323.855141204] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409323.855192321] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.738210221] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.232475 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.804632913] [controller_manager]: Overrun might occur, Total time : 2596.895 us (Expected < 2000.000 us) --> Read time : 13.135 us, Update time : 2582.237 us, Write time : 1.523 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409324.854939402] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.854985470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.855082794] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409325.855139452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.855056084] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409326.855109715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.854848193] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.854900312] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409328.855001595] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409328.855048754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409329.855052657] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409329.855100838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.678223647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.246081 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409330.855031462] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.855082169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.858219359] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.241863 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409331.858556786] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.858575231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.898167602] [controller_manager]: Overrun might occur, Total time : 2125.560 us (Expected < 2000.000 us) --> Read time : 15.640 us, Update time : 2108.277 us, Write time : 1.643 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409332.855033873] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409332.855110619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409333.855163547] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409333.855239060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.644221367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.243630 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409334.856865088] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.856899333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409335.855073861] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.855123716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409336.854994002] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409336.855044108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409337.854917940] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409337.854990809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409338.854997609] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409338.855049517] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409339.854978444] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.855031655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409340.854946146] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409340.854991112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409341.855069902] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409341.855133613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.855030043] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.855088283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409265.263090998] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.713339023] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:30 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:60145 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:60145 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.402644087] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.402700184] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.402711756] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.409908716] [moveit_2533370578.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.412109117] [moveit_2533370578.moveit.ros.rdf_loader]: Loaded robot model in 0.00203912 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.412141308] [moveit_2533370578.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.412151637] [moveit_2533370578.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409274.425153708] [moveit_2533370578.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.427943008] [moveit_2533370578.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.482222837] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.482347303] [moveit_2533370578.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483198170] [moveit_2533370578.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483662683] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483678102] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483788482] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484138427] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484197670] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484879561] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484894129] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.485428534] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.485933013] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409274.486267238] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409274.486289971] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.996951404] [moveit_2533370578.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.997829577] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999298904] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999314924] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999567334] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999579918] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999601980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999606539] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999617009] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.000486571] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.002854901] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.002868197] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.005024223] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.005039031] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.005709815] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.031011302] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.034492758] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.034671978] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.034697547] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.035510742] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:36] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  Generated 2 grasp options for object Box1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  IK accepted: 2 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019170695] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019279131] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019300111] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019324658] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409279.019427403] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019959023] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.020076476] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.046283462] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.046829069] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.047552239] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048809317] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048833904] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048852910] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048866256] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048898347] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050387992] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050470579] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050505686] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050666902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.051611853] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.051630799] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409280.352492091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409280.358335540] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139627241] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139710138] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139722472] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139735567] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409312.139927632] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.140204899] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.140345817] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.157245027] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.157400303] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.157844907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159879733] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159899421] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159913828] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159921002] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159937012] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160386456] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160419599] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160441090] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160580445] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.161194666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.161207751] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.761542234] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.766341108] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:43] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.176302110] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.313757967] [moveit_2533370578.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_box_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py::test_pick_and_place_box_by_position[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... +=================== 1 failed, 1 warning in 99.28s (0:01:39) ==================== + + +12:36:05.51 [INFO] Long running tasks: + 94.88s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests + 207.31s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:36:11.50 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests +12:36:11.51 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-FvhLSf +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py::test_pick_and_place_box_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:35:52 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-31-963480-DESKTOP-HG3Q5KV-6265 (testutils.py:33) + +2026-05-10 12:35:52 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [6281] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [6282] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [6283] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-4]: process started with pid [6284] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-5]: process started with pid [6285] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.329925081] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409272.331945475] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.333641632] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.337053290] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.337077747] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.337083428] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409272.337193001] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.343995090] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.346450863] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.346776472] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.346838820] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.353399575] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.354941761] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.354976227] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355004410] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355010562] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355080435] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355129328] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.626530165] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.878841549] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.878904669] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.881939675] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.900703179] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901601741] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901858910] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901925636] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901955643] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.906793816] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.912640647] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.912685732] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.913635978] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.924871857] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.925162501] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.925838705] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.928734081] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.928769008] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.929491025] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.940629284] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.940929920] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.941450313] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.942976909] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.943030631] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.943711665] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.954907434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.955230844] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.955807850] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.959315705] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.959358546] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.963699521] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.976727820] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.977199813] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.987156712] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.987204092] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.987516896] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409273.000948288] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.001255181] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.007310707] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.007353098] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.007646786] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409273.020946116] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.021266675] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.025441528] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.025491613] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.026093101] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409273.038553997] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.038858491] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.126696651] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 6285] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.379275769] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.379330122] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.380690707] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.394723787] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.395845954] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.395933761] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.401195515] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.401722345] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.404717547] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.405672375] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.405711700] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.406119194] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.420620754] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.420921836] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.422147134] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409273.431053132] [controller_manager]: Overrun might occur, Total time : 7525.935 us (Expected < 2000.000 us) --> Read time : 12.804 us, Update time : 7510.366 us (Switch time : 7462.586 us (Switch chained mode time : 0.391 us, perform mode change time : 9.448 us, Activation time : 7436.325 us, Deactivation time : 0.151 us)), Write time : 2.765 us (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409273.431121281] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.703969 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.431064904] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.432907065] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.433971498] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.434007326] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.434254015] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.447319872] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.448813074] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.449144188] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.449196708] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.452251483] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.453674056] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.456849459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.458025634] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.458067113] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.459388975] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.474951665] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.475328386] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.477494728] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.480435371] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.481696237] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.485038552] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.486608816] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.486658070] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.487013966] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.502579623] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.502923326] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.506399119] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.507632233] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.510655883] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.511711604] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.511747753] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.512122866] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.525038979] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.536880769] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537237812] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537363030] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537374592] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537381324] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.538954524] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.547717656] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.549792107] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.552752131] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 6284] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409274.713334240] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6282] (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409275.017849728] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409275.017907949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.014267187] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.850136 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.226037963] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.621102 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409280.135848182] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431030 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.456654393] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.456706021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.700931158] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.514217 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.458205319] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.458232781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.978260447] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.843536 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.456894199] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409283.456943392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409284.456937511] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409284.456995892] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409285.456809418] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409285.456865655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409286.457407934] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.457456346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.494638366] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.221635 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409287.456878248] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.456934024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409287.554664337] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.678258065] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.841174 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.804411943] [controller_manager]: Overrun might occur, Total time : 2932.872 us (Expected < 2000.000 us) --> Read time : 16.721 us, Update time : 2914.288 us, Write time : 1.863 us (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.105762133] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.458989723] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.459043976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.705270125] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.705328696] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.762975467] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.763041383] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409289.456909119] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.456984462] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409290.011561700] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.222293757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.876945 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409290.459218180] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.459250652] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409291.457301783] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.457354864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.469718441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.301680 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409292.456942239] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.456988617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.750844168] [controller_manager]: Overrun might occur, Total time : 7345.348 us (Expected < 2000.000 us) --> Read time : 19.958 us, Update time : 7321.342 us, Write time : 4.048 us (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.750920232] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.503010 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409293.456899799] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.456961887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.906503706] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.086995 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.456823550] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.456896378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.456883594] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.456931315] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409296.456853328] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.456894546] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409297.456814830] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409297.456854906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.189558291] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141500 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409298.456792497] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.456869654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409299.456785134] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409299.456830751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409300.456745295] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.456796742] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409301.456772065] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409301.456828482] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409302.456927025] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409302.456983502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409303.456880051] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409303.456930537] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409304.456874385] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409304.456935851] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409305.456867453] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.456917578] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409306.456710354] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409306.456761502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409307.456807759] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409307.456848367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409308.456821706] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409308.456878304] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409309.456806281] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409309.456860815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409310.456846162] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409310.456910233] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409311.456774050] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.456822341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.456710107] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409312.456761494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409313.456929463] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409313.456985369] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409314.456781815] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409314.456836720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409315.456863355] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409315.456937877] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409316.456894444] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409316.456948477] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409317.456721782] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409317.456759183] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409318.456889265] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409318.456948357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409319.456867453] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409319.456928449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409320.062350915] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409320.456752982] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409320.456811363] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409320.613377322] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.456923827] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409321.456976617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409321.684114166] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.684174421] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.720373618] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.720439263] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409322.329573081] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409322.456987543] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409322.457062946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409323.456906410] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409323.456961234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409324.456881137] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.456929940] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.812052586] [controller_manager]: Overrun might occur, Total time : 2394.150 us (Expected < 2000.000 us) --> Read time : 39.095 us, Update time : 2353.372 us, Write time : 1.683 us (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.812083995] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.667335 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.456770823] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409325.456820367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.456799753] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409326.456856471] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.456752213] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.456799754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409328.456712729] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409328.456755891] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409329.456643592] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409329.456699288] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409330.456807654] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.456861516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.678223085] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.806454 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409331.456796058] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.456844550] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.854231276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.814545 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409332.456737090] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409332.456788347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409333.456848026] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409333.456901898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409334.456713498] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.456765557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.644356418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.939747 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409335.458709028] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.458746379] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.514190965] [controller_manager]: Overrun might occur, Total time : 2468.540 us (Expected < 2000.000 us) --> Read time : 13.917 us, Update time : 2452.950 us, Write time : 1.673 us (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409336.456834818] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409336.456887839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409337.456820339] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409337.456874702] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409338.456797879] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409338.456867762] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409339.456795070] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.456845236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409340.456747072] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409340.456795705] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409341.456838050] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409341.456909075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.456828362] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.456876663] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.456679776] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409343.456729300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409344.456726168] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409344.456777075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.456877551] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409345.456930442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409346.456721266] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409346.456791680] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409347.456743598] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409347.456795496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409348.456697075] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409348.456746730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409349.456648931] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409349.456693606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409350.456673349] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409350.456721390] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409351.456703350] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409351.456752333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409352.456646819] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409352.456696313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [1778409274.904707518] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] [INFO] [1778409284.188595033] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:35:52 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] * Running on http://127.0.0.1:33273 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] * Running on http://172.30.43.138:33273 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:44 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.015243124] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.015287419] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.015297297] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.022040389] [moveit_1358702927.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.024506086] [moveit_1358702927.moveit.ros.rdf_loader]: Loaded robot model in 0.00230985 seconds (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.024536974] [moveit_1358702927.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.024546983] [moveit_1358702927.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [WARN] [1778409285.036991738] [moveit_1358702927.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.040317718] [moveit_1358702927.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.107739013] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.107916620] [moveit_1358702927.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.108931799] [moveit_1358702927.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.109503946] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.109520237] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.109653711] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110098927] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110178598] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110894278] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110910208] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.111408254] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.111891743] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [WARN] [1778409285.112150485] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [ERROR] [1778409285.112166646] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.143058463] [moveit_1358702927.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.143697537] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.144955147] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.144970366] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145264364] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145277239] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145298820] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145302727] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145311414] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.146294992] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.148497537] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.148511363] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.150697727] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.150714098] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.151371919] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.177133053] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.180569552] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.180794159] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.180825669] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.181678219] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:46] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  Generated 2 grasp options for object Box1. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  IK accepted: 2 grasp options. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733000647] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733119372] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733137447] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733155411] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [WARN] [1778409288.733221236] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733715816] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733866151] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.752424549] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.752860999] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.753311334] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Executing plan (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759531328] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759571495] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759601572] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759613584] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759639564] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.761896362] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.761963539] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.761991372] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.762129194] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.763878604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.763898422] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409290.014115276] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409290.019725943] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:05 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698886720] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698960219] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698971431] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698983223] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [WARN] [1778409321.699120544] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.699372122] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.699416577] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.717973357] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.718168809] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.718593285] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Executing plan (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719126153] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719149427] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719165317] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719172511] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719189303] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719844547] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719879364] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719902668] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.720052783] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.720686888] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.720700604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409322.371186893] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409322.375782694] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:37 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:52] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409352.952259602] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:36:07 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:36:07 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:36:08 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:36:10 [ ERROR] [INFO] [1778409368.079316218] [moveit_1358702927.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________ test_pick_and_place_box_by_id[ur5e] ______________________ + +start_processes = Urls(ros_domain_id='214', robot_url='http://0.0.0.0:33273', storage_url='http://127.0.0.1:35055') + + @pytest.mark.timeout(400) + def test_pick_and_place_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Box1. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-31-963480-DESKTOP-HG3Q5KV-6265 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [6281] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [6282] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [6283] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [6284] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [6285] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.329925081] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409272.331945475] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.333641632] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.337053290] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.337077747] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.337083428] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409272.337193001] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.343995090] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.346450863] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.346776472] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.346838820] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.353399575] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.354941761] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.354976227] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355004410] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355010562] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355080435] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355129328] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.626530165] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.878841549] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.878904669] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.881939675] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.900703179] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901601741] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901858910] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901925636] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901955643] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.906793816] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.912640647] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.912685732] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.913635978] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.924871857] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.925162501] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.925838705] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.928734081] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.928769008] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.929491025] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.940629284] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.940929920] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.941450313] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.942976909] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.943030631] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.943711665] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.954907434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.955230844] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.955807850] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.959315705] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.959358546] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.963699521] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.976727820] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.977199813] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.987156712] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.987204092] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.987516896] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409273.000948288] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.001255181] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.007310707] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.007353098] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.007646786] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409273.020946116] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.021266675] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.025441528] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.025491613] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.026093101] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409273.038553997] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.038858491] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.126696651] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 6285] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.379275769] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.379330122] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.380690707] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.394723787] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.395845954] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.395933761] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.401195515] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.401722345] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.404717547] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.405672375] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.405711700] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.406119194] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.420620754] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.420921836] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.422147134] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409273.431053132] [controller_manager]: Overrun might occur, Total time : 7525.935 us (Expected < 2000.000 us) --> Read time : 12.804 us, Update time : 7510.366 us (Switch time : 7462.586 us (Switch chained mode time : 0.391 us, perform mode change time : 9.448 us, Activation time : 7436.325 us, Deactivation time : 0.151 us)), Write time : 2.765 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409273.431121281] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.703969 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.431064904] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.432907065] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.433971498] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.434007326] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.434254015] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.447319872] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.448813074] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.449144188] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.449196708] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.452251483] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.453674056] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.456849459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.458025634] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.458067113] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.459388975] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.474951665] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.475328386] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.477494728] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.480435371] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.481696237] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.485038552] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.486608816] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.486658070] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.487013966] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.502579623] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.502923326] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.506399119] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.507632233] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.510655883] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.511711604] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.511747753] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.512122866] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.525038979] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.536880769] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537237812] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537363030] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537374592] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537381324] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.538954524] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.547717656] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.549792107] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.552752131] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 6284] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409274.713334240] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6282] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409275.017849728] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409275.017907949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.014267187] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.850136 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.226037963] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.621102 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409280.135848182] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431030 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.456654393] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.456706021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.700931158] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.514217 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.458205319] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.458232781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.978260447] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.843536 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.456894199] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409283.456943392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409284.456937511] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409284.456995892] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409285.456809418] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409285.456865655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409286.457407934] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.457456346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.494638366] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.221635 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409287.456878248] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.456934024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409287.554664337] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.678258065] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.841174 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.804411943] [controller_manager]: Overrun might occur, Total time : 2932.872 us (Expected < 2000.000 us) --> Read time : 16.721 us, Update time : 2914.288 us, Write time : 1.863 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.105762133] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.458989723] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.459043976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.705270125] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.705328696] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.762975467] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.763041383] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409289.456909119] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.456984462] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409290.011561700] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.222293757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.876945 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409290.459218180] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.459250652] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409291.457301783] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.457354864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.469718441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.301680 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409292.456942239] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.456988617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.750844168] [controller_manager]: Overrun might occur, Total time : 7345.348 us (Expected < 2000.000 us) --> Read time : 19.958 us, Update time : 7321.342 us, Write time : 4.048 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.750920232] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.503010 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409293.456899799] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.456961887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.906503706] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.086995 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.456823550] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.456896378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.456883594] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.456931315] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409296.456853328] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.456894546] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409297.456814830] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409297.456854906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.189558291] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141500 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409298.456792497] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.456869654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409299.456785134] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409299.456830751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409300.456745295] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.456796742] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409301.456772065] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409301.456828482] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409302.456927025] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409302.456983502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409303.456880051] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409303.456930537] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409304.456874385] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409304.456935851] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409305.456867453] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.456917578] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409306.456710354] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409306.456761502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409307.456807759] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409307.456848367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409308.456821706] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409308.456878304] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409309.456806281] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409309.456860815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409310.456846162] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409310.456910233] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409311.456774050] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.456822341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.456710107] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409312.456761494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409313.456929463] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409313.456985369] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409314.456781815] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409314.456836720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409315.456863355] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409315.456937877] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409316.456894444] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409316.456948477] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409317.456721782] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409317.456759183] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409318.456889265] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409318.456948357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409319.456867453] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409319.456928449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409320.062350915] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409320.456752982] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409320.456811363] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409320.613377322] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.456923827] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409321.456976617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409321.684114166] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.684174421] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.720373618] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.720439263] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409322.329573081] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409322.456987543] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409322.457062946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409323.456906410] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409323.456961234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409324.456881137] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.456929940] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.812052586] [controller_manager]: Overrun might occur, Total time : 2394.150 us (Expected < 2000.000 us) --> Read time : 39.095 us, Update time : 2353.372 us, Write time : 1.683 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.812083995] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.667335 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.456770823] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409325.456820367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.456799753] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409326.456856471] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.456752213] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.456799754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409328.456712729] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409328.456755891] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409329.456643592] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409329.456699288] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409330.456807654] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.456861516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.678223085] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.806454 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409331.456796058] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.456844550] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.854231276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.814545 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409332.456737090] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409332.456788347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409333.456848026] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409333.456901898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409334.456713498] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.456765557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.644356418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.939747 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409335.458709028] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.458746379] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.514190965] [controller_manager]: Overrun might occur, Total time : 2468.540 us (Expected < 2000.000 us) --> Read time : 13.917 us, Update time : 2452.950 us, Write time : 1.673 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409336.456834818] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409336.456887839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409337.456820339] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409337.456874702] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409338.456797879] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409338.456867762] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409339.456795070] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.456845236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409340.456747072] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409340.456795705] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409341.456838050] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409341.456909075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.456828362] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.456876663] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.456679776] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409343.456729300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409344.456726168] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409344.456777075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.456877551] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409345.456930442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409346.456721266] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409346.456791680] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409347.456743598] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409347.456795496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409348.456697075] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409348.456746730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409349.456648931] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409349.456693606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409350.456673349] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409350.456721390] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409351.456703350] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409351.456752333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409352.456646819] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409352.456696313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.904707518] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409284.188595033] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:33273 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:33273 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.015243124] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.015287419] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.015297297] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.022040389] [moveit_1358702927.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.024506086] [moveit_1358702927.moveit.ros.rdf_loader]: Loaded robot model in 0.00230985 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.024536974] [moveit_1358702927.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.024546983] [moveit_1358702927.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409285.036991738] [moveit_1358702927.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.040317718] [moveit_1358702927.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.107739013] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.107916620] [moveit_1358702927.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.108931799] [moveit_1358702927.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.109503946] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.109520237] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.109653711] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110098927] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110178598] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110894278] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110910208] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.111408254] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.111891743] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409285.112150485] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409285.112166646] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.143058463] [moveit_1358702927.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.143697537] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.144955147] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.144970366] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145264364] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145277239] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145298820] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145302727] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145311414] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.146294992] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.148497537] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.148511363] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.150697727] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.150714098] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.151371919] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.177133053] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.180569552] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.180794159] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.180825669] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.181678219] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:46] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Generated 2 grasp options for object Box1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  IK accepted: 2 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733000647] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733119372] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733137447] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733155411] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409288.733221236] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733715816] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733866151] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.752424549] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.752860999] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.753311334] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759531328] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759571495] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759601572] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759613584] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759639564] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.761896362] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.761963539] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.761991372] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.762129194] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.763878604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.763898422] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409290.014115276] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409290.019725943] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698886720] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698960219] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698971431] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698983223] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409321.699120544] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.699372122] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.699416577] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.717973357] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.718168809] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.718593285] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719126153] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719149427] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719165317] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719172511] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719189303] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719844547] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719879364] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719902668] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.720052783] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.720686888] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.720700604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409322.371186893] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409322.375782694] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:52] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409352.952259602] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:07 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:07 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:08 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409368.079316218] [moveit_1358702927.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_box_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py::test_pick_and_place_box_by_id[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... +=================== 1 failed, 1 warning in 99.71s (0:01:39) ==================== + + +12:36:35.57 [INFO] Long running tasks: + 71.05s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 237.37s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:36:35.69 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests +12:36:35.69 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-7OBLoJ +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py::test_pick_up_box_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:36:17 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-02-437060-DESKTOP-HG3Q5KV-7847 (testutils.py:33) + +2026-05-10 12:36:17 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [7851] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [7852] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [7853] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-4]: process started with pid [7854] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-5]: process started with pid [7855] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409362.698293946] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.701067742] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.703443301] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.706279775] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.706312116] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.706318499] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409362.706422085] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.714017898] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.716407630] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.716780859] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.716849529] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.724328220] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725375660] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725403473] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725426928] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725431446] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725478085] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725509765] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409362.947512446] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.199660102] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.199713924] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.202547148] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.223923139] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.224956984] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.225118901] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.225155120] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.225177673] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.229565932] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.235852150] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.235899280] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.236987960] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.247926694] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.248157157] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.248964270] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.251922977] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.251970016] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.252611225] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.263533594] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.263754343] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.264252299] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.265892857] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.265927993] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.266545006] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.277858744] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.278077075] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.278662437] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.280004481] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.280043625] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.284537915] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.297601527] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.297842615] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.306181274] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.306223264] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.306546919] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.319885525] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.320108764] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.326020832] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.326056981] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.326399803] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.339696263] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.339918735] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.340517734] [freedrive_mode_controller]: Freedrive mode has been enabled successfully. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.342283970] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.342312734] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.342746799] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.355878766] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.356165686] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.453364310] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 7855] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.705919862] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.705981198] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.707495821] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.721981680] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.723162905] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.723228559] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.728226161] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.728986991] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.731738520] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.732814379] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.732857201] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.733206363] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.747728913] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.748057638] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.749629644] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409363.760097129] [controller_manager]: Overrun might occur, Total time : 9329.893 us (Expected < 2000.000 us) --> Read time : 13.095 us, Update time : 9313.642 us (Switch time : 9266.693 us (Switch chained mode time : 0.390 us, perform mode change time : 12.183 us, Activation time : 9238.179 us, Deactivation time : 0.141 us)), Write time : 3.156 us (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409363.760182657] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.516241 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.760105751] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.761810936] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.762684896] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.762724191] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.763112749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.775962826] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.777994653] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.778305427] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.778370631] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.781514791] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.782878346] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.785912918] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.786971243] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.787006510] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.788137395] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.803740166] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.804125614] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.806453497] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.807545877] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.808855481] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.811766713] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.812940608] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.812975554] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.813348814] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.827875823] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.828305714] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.831666370] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.832879039] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.835639034] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.836887059] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.836929179] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.837256732] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.850284111] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.866050885] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866580216] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866707888] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866726684] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866737024] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.868270081] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.875743106] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.877122367] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.879897589] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 7854] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409365.249884154] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7852] (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409365.541017098] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409365.541067364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.477540211] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.873825 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.726213048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.547083 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.850926975] [controller_manager]: Overrun might occur, Total time : 2078.966 us (Expected < 2000.000 us) --> Read time : 176.991 us, Update time : 1900.301 us, Write time : 1.674 us (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.519038159] [controller_manager]: Overrun might occur, Total time : 2313.550 us (Expected < 2000.000 us) --> Read time : 138.918 us, Update time : 2172.007 us, Write time : 2.625 us (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.519103603] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.436716 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409372.089874163] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409372.089927534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.046941930] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.046994119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.046863138] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409374.046916770] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409375.046791576] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409375.046841932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409376.046838342] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409376.046883147] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409377.046990234] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.047059516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [1778409365.411366456] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] [INFO] [1778409375.566907179] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:36:17 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:10 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] * Running on http://127.0.0.1:47847 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] * Running on http://172.30.43.138:47847 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:15 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.499689743] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.499754015] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.499771038] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.505902168] [moveit_1079793702.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.508078764] [moveit_1079793702.moveit.ros.rdf_loader]: Loaded robot model in 0.00203154 seconds (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.508134931] [moveit_1079793702.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.508152684] [moveit_1079793702.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [WARN] [1778409375.519694890] [moveit_1079793702.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.522656955] [moveit_1079793702.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.580353904] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.580498830] [moveit_1079793702.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.581597327] [moveit_1079793702.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582155398] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582172861] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582278827] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582599016] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582677956] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.583252162] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.583267541] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.583709040] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.584193540] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [WARN] [1778409375.584469935] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [ERROR] [1778409375.584490324] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.659926833] [moveit_1079793702.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.660599331] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.661708439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.661722726] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662062392] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662097438] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662126804] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662131573] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662152443] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662841953] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.665617416] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.665631613] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.667703350] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.667721444] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.668501887] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.696451154] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.699843500] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.700016248] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.700042528] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.700677414] [moveit_1079793702.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:17] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:17] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409377.217789605] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:32 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:32 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:32 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:36:35 [ ERROR] [INFO] [1778409392.346505595] [moveit_1079793702.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_________________________ test_pick_up_box_by_id[ur5e] _________________________ + +start_processes = Urls(ros_domain_id='162', robot_url='http://0.0.0.0:47847', storage_url='http://127.0.0.1:55709') + + @pytest.mark.timeout(400) + def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Box("Box1", 0.2, 0.2, 0.2) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:27: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Box1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-02-437060-DESKTOP-HG3Q5KV-7847 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [7851] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [7852] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [7853] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [7854] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [7855] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409362.698293946] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.701067742] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.703443301] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.706279775] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.706312116] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.706318499] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409362.706422085] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.714017898] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.716407630] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.716780859] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.716849529] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.724328220] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725375660] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725403473] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725426928] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725431446] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725478085] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725509765] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409362.947512446] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.199660102] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.199713924] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.202547148] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.223923139] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.224956984] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.225118901] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.225155120] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.225177673] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.229565932] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.235852150] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.235899280] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.236987960] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.247926694] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.248157157] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.248964270] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.251922977] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.251970016] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.252611225] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.263533594] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.263754343] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.264252299] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.265892857] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.265927993] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.266545006] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.277858744] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.278077075] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.278662437] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.280004481] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.280043625] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.284537915] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.297601527] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.297842615] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.306181274] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.306223264] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.306546919] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.319885525] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.320108764] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.326020832] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.326056981] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.326399803] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.339696263] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.339918735] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.340517734] [freedrive_mode_controller]: Freedrive mode has been enabled successfully. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.342283970] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.342312734] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.342746799] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.355878766] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.356165686] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.453364310] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 7855] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.705919862] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.705981198] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.707495821] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.721981680] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.723162905] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.723228559] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.728226161] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.728986991] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.731738520] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.732814379] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.732857201] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.733206363] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.747728913] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.748057638] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.749629644] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409363.760097129] [controller_manager]: Overrun might occur, Total time : 9329.893 us (Expected < 2000.000 us) --> Read time : 13.095 us, Update time : 9313.642 us (Switch time : 9266.693 us (Switch chained mode time : 0.390 us, perform mode change time : 12.183 us, Activation time : 9238.179 us, Deactivation time : 0.141 us)), Write time : 3.156 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409363.760182657] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.516241 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.760105751] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.761810936] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.762684896] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.762724191] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.763112749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.775962826] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.777994653] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.778305427] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.778370631] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.781514791] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.782878346] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.785912918] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.786971243] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.787006510] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.788137395] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.803740166] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.804125614] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.806453497] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.807545877] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.808855481] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.811766713] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.812940608] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.812975554] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.813348814] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.827875823] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.828305714] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.831666370] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.832879039] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.835639034] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.836887059] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.836929179] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.837256732] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.850284111] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.866050885] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866580216] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866707888] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866726684] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866737024] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.868270081] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.875743106] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.877122367] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.879897589] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 7854] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409365.249884154] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7852] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409365.541017098] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409365.541067364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.477540211] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.873825 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.726213048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.547083 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.850926975] [controller_manager]: Overrun might occur, Total time : 2078.966 us (Expected < 2000.000 us) --> Read time : 176.991 us, Update time : 1900.301 us, Write time : 1.674 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.519038159] [controller_manager]: Overrun might occur, Total time : 2313.550 us (Expected < 2000.000 us) --> Read time : 138.918 us, Update time : 2172.007 us, Write time : 2.625 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.519103603] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.436716 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409372.089874163] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409372.089927534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.046941930] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.046994119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.046863138] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409374.046916770] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409375.046791576] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409375.046841932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409376.046838342] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409376.046883147] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409377.046990234] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.047059516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409365.411366456] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.566907179] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:47847 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:47847 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.499689743] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.499754015] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.499771038] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.505902168] [moveit_1079793702.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.508078764] [moveit_1079793702.moveit.ros.rdf_loader]: Loaded robot model in 0.00203154 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.508134931] [moveit_1079793702.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.508152684] [moveit_1079793702.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409375.519694890] [moveit_1079793702.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.522656955] [moveit_1079793702.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.580353904] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.580498830] [moveit_1079793702.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.581597327] [moveit_1079793702.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582155398] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582172861] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582278827] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582599016] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582677956] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.583252162] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.583267541] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.583709040] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.584193540] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409375.584469935] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409375.584490324] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.659926833] [moveit_1079793702.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.660599331] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.661708439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.661722726] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662062392] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662097438] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662126804] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662131573] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662152443] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662841953] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.665617416] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.665631613] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.667703350] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.667721444] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.668501887] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.696451154] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.699843500] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.700016248] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.700042528] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.700677414] [moveit_1079793702.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:17] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:17] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409377.217789605] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409392.346505595] [moveit_1079793702.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:13 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_box_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py::test_pick_up_box_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 33.36s ========================= + + +12:37:05.62 [INFO] Long running tasks: + 101.09s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 267.42s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:37:08.86 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests +12:37:08.86 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-cmgKAE +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py::test_pick_up_mesh_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:36:50 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-36-854431-DESKTOP-HG3Q5KV-8758 (testutils.py:33) + +2026-05-10 12:36:50 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [8762] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [8763] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [8764] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-4]: process started with pid [8765] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-5]: process started with pid [8766] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [robot_state_publisher-2] [INFO] [1778409397.162957392] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.168983002] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.171939720] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.174288158] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.174318756] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.174324337] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.174428915] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.343808993] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.346508607] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.346891495] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.346964684] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.355604571] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357018517] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357056870] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357081106] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357086326] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357149115] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357187167] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.456317492] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.708408366] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.708467237] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.711002745] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.724061493] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.725217886] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.725358272] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.734219074] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.734951275] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.737781293] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.738611045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.738655970] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.743747895] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.755901879] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.756164708] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.757312880] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.768043784] [controller_manager]: Overrun might occur, Total time : 9306.634 us (Expected < 2000.000 us) --> Read time : 13.035 us, Update time : 9291.545 us (Switch time : 9249.525 us (Switch chained mode time : 0.411 us, perform mode change time : 10.840 us, Activation time : 9224.267 us, Deactivation time : 0.220 us)), Write time : 2.054 us (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.768101695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.447075 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.768058453] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.770165011] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.771071323] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.771161043] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.771552898] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.781835135] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.783901827] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.784134785] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.784171615] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.785560369] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.786844014] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.789675353] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.790627451] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.790660604] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.791993663] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.805917535] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.806209870] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.808192132] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.809531497] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.810869876] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.813793790] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.814699977] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.814729242] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.815009956] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.828079596] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.828333007] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.831595827] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.832868044] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.835897791] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.836877863] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.836919963] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.837206447] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.847869208] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.859939803] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860149882] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860321649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860345444] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860369770] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.862742820] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.869799007] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.870988457] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.873960789] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409397.960850175] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 8765] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.213386105] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.213434967] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.213795773] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.237905859] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239094683] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239234960] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239267021] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239279785] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.240494183] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.246001036] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.246052414] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.247493256] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.261808677] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.262109004] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.263047837] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.266109619] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.266156588] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.267009896] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.281629349] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.281937509] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.282416009] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.284199087] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.284233482] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.284972141] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.299837702] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.300129847] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.300617524] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.304179016] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.304210977] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.304541575] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.319994914] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.320335231] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.330359668] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.330420333] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.330851753] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.345875269] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.346168286] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.352481447] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.352510853] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.352755147] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.367855264] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.368210830] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.372361136] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.372399128] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.372668310] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.388003935] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.388301906] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 8766] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [robot_state_publisher-2] [INFO] [1778409399.647017065] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8763] (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409399.937946051] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409399.938000945] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.042242378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.588370 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.866249029] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.594821 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.092568163] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.092623027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.341779581] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.341841449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409408.342128646] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409408.342208658] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409409.341795488] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409409.341856474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.342059747] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409410.342125331] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [1778409399.824482336] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] [INFO] [1778409410.431215087] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:36:50 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] * Running on http://127.0.0.1:58965 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] * Running on http://172.30.43.138:58965 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.493135262] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.493178655] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.493189475] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.500468376] [moveit_2109978589.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.503447206] [moveit_2109978589.moveit.ros.rdf_loader]: Loaded robot model in 0.00278104 seconds (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.503488414] [moveit_2109978589.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.503505937] [moveit_2109978589.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [WARN] [1778409409.517172621] [moveit_2109978589.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.520555374] [moveit_2109978589.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.583144240] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.583316337] [moveit_2109978589.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.584473575] [moveit_2109978589.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585171376] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585188408] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585313431] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585785317] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585873335] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.586653617] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.586668736] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.587360320] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.588086701] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [WARN] [1778409409.588369849] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [ERROR] [1778409409.588386601] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.745309033] [moveit_2109978589.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.746133379] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747376030] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747393664] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747739020] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747753958] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747783745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747794606] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747822128] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.748466172] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.750997893] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.751013703] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.753529182] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.753544511] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.754616788] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.788634115] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.793023476] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.793248463] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.793279582] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.794424843] [moveit_2109978589.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:50] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:50] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409410.563385788] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:37:05 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:37:05 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:37:05 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:37:08 [ ERROR] [INFO] [1778409425.712762606] [moveit_2109978589.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________ test_pick_up_mesh_by_position[ur5e] ______________________ + +start_processes = Urls(ros_domain_id='147', robot_url='http://0.0.0.0:58965', storage_url='http://127.0.0.1:33125') + + @pytest.mark.timeout(321) + def test_pick_up_mesh_by_position(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" +  + storage_client.URL = start_processes.storage_url +  + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) +  + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Mesh("Mesh1", MESH_ASSET_ID) +> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:42: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2/exceptions/helpers.py:20: in wrapper + return func(*args, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^ +src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable + params, model_type = _collision_params(model, mesh_parameters) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +model = 'Mesh1', mesh_parameters = None + + def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: +> params = model.to_dict() + ^^^^^^^^^^^^^ +E AttributeError: 'str' object has no attribute 'to_dict' + +src/python/arcor2_scene_data/scene_service.py:72: AttributeError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-36-854431-DESKTOP-HG3Q5KV-8758 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [8762] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [8763] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [8764] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [8765] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [8766] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409397.162957392] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.168983002] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.171939720] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.174288158] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.174318756] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.174324337] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.174428915] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.343808993] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.346508607] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.346891495] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.346964684] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.355604571] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357018517] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357056870] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357081106] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357086326] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357149115] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357187167] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.456317492] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.708408366] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.708467237] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.711002745] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.724061493] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.725217886] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.725358272] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.734219074] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.734951275] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.737781293] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.738611045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.738655970] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.743747895] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.755901879] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.756164708] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.757312880] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.768043784] [controller_manager]: Overrun might occur, Total time : 9306.634 us (Expected < 2000.000 us) --> Read time : 13.035 us, Update time : 9291.545 us (Switch time : 9249.525 us (Switch chained mode time : 0.411 us, perform mode change time : 10.840 us, Activation time : 9224.267 us, Deactivation time : 0.220 us)), Write time : 2.054 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.768101695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.447075 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.768058453] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.770165011] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.771071323] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.771161043] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.771552898] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.781835135] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.783901827] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.784134785] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.784171615] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.785560369] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.786844014] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.789675353] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.790627451] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.790660604] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.791993663] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.805917535] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.806209870] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.808192132] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.809531497] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.810869876] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.813793790] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.814699977] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.814729242] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.815009956] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.828079596] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.828333007] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.831595827] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.832868044] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.835897791] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.836877863] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.836919963] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.837206447] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.847869208] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.859939803] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860149882] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860321649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860345444] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860369770] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.862742820] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.869799007] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.870988457] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.873960789] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409397.960850175] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 8765] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.213386105] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.213434967] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.213795773] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.237905859] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239094683] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239234960] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239267021] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239279785] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.240494183] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.246001036] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.246052414] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.247493256] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.261808677] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.262109004] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.263047837] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.266109619] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.266156588] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.267009896] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.281629349] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.281937509] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.282416009] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.284199087] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.284233482] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.284972141] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.299837702] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.300129847] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.300617524] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.304179016] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.304210977] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.304541575] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.319994914] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.320335231] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.330359668] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.330420333] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.330851753] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.345875269] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.346168286] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.352481447] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.352510853] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.352755147] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.367855264] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.368210830] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.372361136] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.372399128] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.372668310] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.388003935] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.388301906] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 8766] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409399.647017065] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8763] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409399.937946051] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409399.938000945] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.042242378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.588370 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.866249029] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.594821 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.092568163] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.092623027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.341779581] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.341841449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409408.342128646] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409408.342208658] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409409.341795488] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409409.341856474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.342059747] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409410.342125331] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409399.824482336] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.431215087] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:58965 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:58965 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.493135262] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.493178655] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.493189475] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.500468376] [moveit_2109978589.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.503447206] [moveit_2109978589.moveit.ros.rdf_loader]: Loaded robot model in 0.00278104 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.503488414] [moveit_2109978589.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.503505937] [moveit_2109978589.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409409.517172621] [moveit_2109978589.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.520555374] [moveit_2109978589.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.583144240] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.583316337] [moveit_2109978589.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.584473575] [moveit_2109978589.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585171376] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585188408] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585313431] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585785317] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585873335] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.586653617] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.586668736] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.587360320] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.588086701] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409409.588369849] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409409.588386601] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.745309033] [moveit_2109978589.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.746133379] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747376030] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747393664] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747739020] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747753958] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747783745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747794606] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747822128] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.748466172] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.750997893] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.751013703] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.753529182] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.753544511] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.754616788] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.788634115] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.793023476] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.793248463] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.793279582] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.794424843] [moveit_2109978589.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:50] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:50] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.563385788] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409425.712762606] [moveit_2109978589.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:18 + src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_mesh_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py::test_pick_up_mesh_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' +======================== 1 failed, 1 warning in 32.20s ========================= + + +12:37:35.66 [INFO] Long running tasks: + 84.15s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests + 131.14s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 297.46s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:37:58.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests +12:37:58.25 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-gbn5x0 +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py::test_pick_and_place_mesh_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:37:40 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-09-975076-DESKTOP-HG3Q5KV-9294 (testutils.py:33) + +2026-05-10 12:37:40 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [9301] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [9302] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [9303] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-4]: process started with pid [9304] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-5]: process started with pid [9305] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [robot_state_publisher-2] [INFO] [1778409430.283072484] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.290127024] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.293110835] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.295385617] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.295408190] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.295417127] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.295502138] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.460250108] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.463333817] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.463758784] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.463848415] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.473434458] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475031773] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475068273] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475098510] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475105183] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475187319] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475240019] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.575491178] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.827882954] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.827937067] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.830258218] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.842762256] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.843890946] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.844004481] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.851256036] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.852075537] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.854937024] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.855728088] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.855761962] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.860790948] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.875133631] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.875395533] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.876392513] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.888327004] [controller_manager]: Overrun might occur, Total time : 10500.567 us (Expected < 2000.000 us) --> Read time : 12.654 us, Update time : 10485.718 us (Switch time : 10431.786 us (Switch chained mode time : 0.421 us, perform mode change time : 16.111 us, Activation time : 10400.527 us, Deactivation time : 0.320 us)), Write time : 2.195 us (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.888397017] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.681132 ms (missed cycles : 6). (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.888336377] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.890746205] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.891579003] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.891632404] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.892101676] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.902523645] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.904737415] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.905006041] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.905051728] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.906578349] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.907950646] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.910702891] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.911613246] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.911647551] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.912672869] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.926847733] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.927110793] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.929276157] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.930672471] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.931920958] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.934792514] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.935645219] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.935680917] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.935906376] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.948847494] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.949093797] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.952600080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.954013912] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.956894364] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.958003287] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.958038324] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.958328104] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.969527479] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.981286372] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981577581] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981811977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981868745] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981971560] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.984779390] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.992642003] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.994081759] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.997018094] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.083562408] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 9304] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.336052076] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.336110427] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.336471422] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.361236664] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362605320] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362759884] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362797024] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362810289] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.364030217] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.371276932] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.371317229] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.372861593] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.389025791] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.389365848] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.390468733] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.393397488] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.393434449] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.394269941] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.411003271] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.411368926] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.412190507] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.415473695] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.415513420] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.416306347] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.433069809] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.433410166] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.434035424] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.437238225] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.437270206] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.437555177] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.452842175] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.453169652] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.463384456] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.463447686] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.463758938] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.478987382] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.479295043] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.485501135] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.485546211] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.485877835] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.500733118] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.501059497] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.505489947] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.505525755] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.505784477] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.520771271] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.521083360] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 9305] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [robot_state_publisher-2] [INFO] [1778409432.763700913] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9302] (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.073391159] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409433.073445442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.992241141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.526579 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.161177547] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.462914 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.246199523] [controller_manager]: Overrun might occur, Total time : 2426.716 us (Expected < 2000.000 us) --> Read time : 12.864 us, Update time : 2412.239 us, Write time : 1.613 us (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.006216951] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.502268 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409439.434062068] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.434115389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409440.434129650] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409440.434239659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409441.434104073] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409441.434166212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409442.434163204] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409442.434230552] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409443.434155498] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409443.434205553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409444.434259854] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409444.434315530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.434148025] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409445.434202869] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.783414939] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.334387877] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.434180812] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.434229184] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.892924859] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.892967801] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.099077143] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.099173547] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.434130389] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409447.434225630] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.434186935] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409448.434235297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.595842484] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.614335620] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.165297978] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.433943105] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.433998400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.721228677] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.721281176] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.433999883] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409450.434038716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409451.434230884] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409451.434307500] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409452.434199919] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409452.434253361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409453.433989726] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409453.434051784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409454.434154454] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409454.434230248] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409455.434100464] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409455.434166469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409456.434081412] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409456.434132860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409457.434134292] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409457.434197072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409458.433948272] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409458.434003066] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409459.434215665] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409459.434271941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [1778409432.949648868] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] [INFO] [1778409460.036698451] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:37:40 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:18 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] * Running on http://127.0.0.1:46513 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] * Running on http://172.30.43.138:46513 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.396550516] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.396607525] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.396623695] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.400874802] [moveit_1090737569.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.403194570] [moveit_1090737569.moveit.ros.rdf_loader]: Loaded robot model in 0.00216529 seconds (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.403227893] [moveit_1090737569.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.403236991] [moveit_1090737569.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [WARN] [1778409442.415048715] [moveit_1090737569.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.417972099] [moveit_1090737569.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.479782616] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.480010600] [moveit_1090737569.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481120994] [moveit_1090737569.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481598101] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481612919] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481748312] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482148212] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482233524] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482733814] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482749013] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.483157780] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.483591203] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [WARN] [1778409442.483949273] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409442.483970414] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.973274624] [moveit_1090737569.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.974414961] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.975805263] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.975824369] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976189643] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976205373] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976239728] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976250439] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976261259] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976991126] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.979382871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.979395244] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.981620287] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.981636437] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.982358480] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.018995259] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.022934028] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.023103079] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.023132004] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.023812477] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:24] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  Generated 1 grasp options for object Mesh1. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  IK accepted: 1 grasp options. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016513833] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016631015] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016644711] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016663096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [WARN] [1778409447.016731406] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.017212781] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.017309254] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.034331148] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.034818875] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.035063309] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Executing plan (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096413186] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096470325] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096503067] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096511844] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096544025] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098332793] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098379933] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098406062] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098538494] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.099385574] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.099409519] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409448.600248882] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409448.606065885] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:29 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.757943972] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.757994779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758002173] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758011360] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [WARN] [1778409449.758072987] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758319315] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758379840] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.762259084] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.762469064] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.763025205] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.763156835] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [WARN] [1778409459.763303860] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004847 seconds (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.767335099] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.767395965] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:39 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:39] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409460.166867386] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:55 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:55 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:55 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:37:57 [ ERROR] [INFO] [1778409475.300799510] [moveit_1090737569.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________ test_pick_and_place_mesh_by_id[ur5e] _____________________ + +start_processes = Urls(ros_domain_id='48', robot_url='http://0.0.0.0:46513', storage_url='http://127.0.0.1:51823') + + @pytest.mark.timeout(321) + def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" +  + storage_client.URL = start_processes.storage_url +  + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) +  + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:45: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Mesh1. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-09-975076-DESKTOP-HG3Q5KV-9294 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [9301] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [9302] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [9303] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [9304] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [9305] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409430.283072484] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.290127024] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.293110835] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.295385617] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.295408190] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.295417127] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.295502138] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.460250108] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.463333817] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.463758784] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.463848415] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.473434458] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475031773] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475068273] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475098510] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475105183] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475187319] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475240019] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.575491178] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.827882954] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.827937067] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.830258218] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.842762256] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.843890946] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.844004481] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.851256036] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.852075537] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.854937024] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.855728088] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.855761962] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.860790948] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.875133631] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.875395533] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.876392513] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.888327004] [controller_manager]: Overrun might occur, Total time : 10500.567 us (Expected < 2000.000 us) --> Read time : 12.654 us, Update time : 10485.718 us (Switch time : 10431.786 us (Switch chained mode time : 0.421 us, perform mode change time : 16.111 us, Activation time : 10400.527 us, Deactivation time : 0.320 us)), Write time : 2.195 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.888397017] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.681132 ms (missed cycles : 6). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.888336377] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.890746205] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.891579003] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.891632404] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.892101676] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.902523645] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.904737415] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.905006041] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.905051728] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.906578349] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.907950646] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.910702891] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.911613246] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.911647551] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.912672869] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.926847733] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.927110793] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.929276157] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.930672471] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.931920958] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.934792514] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.935645219] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.935680917] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.935906376] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.948847494] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.949093797] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.952600080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.954013912] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.956894364] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.958003287] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.958038324] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.958328104] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.969527479] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.981286372] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981577581] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981811977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981868745] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981971560] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.984779390] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.992642003] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.994081759] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.997018094] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.083562408] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 9304] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.336052076] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.336110427] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.336471422] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.361236664] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362605320] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362759884] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362797024] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362810289] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.364030217] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.371276932] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.371317229] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.372861593] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.389025791] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.389365848] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.390468733] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.393397488] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.393434449] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.394269941] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.411003271] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.411368926] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.412190507] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.415473695] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.415513420] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.416306347] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.433069809] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.433410166] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.434035424] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.437238225] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.437270206] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.437555177] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.452842175] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.453169652] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.463384456] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.463447686] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.463758938] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.478987382] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.479295043] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.485501135] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.485546211] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.485877835] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.500733118] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.501059497] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.505489947] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.505525755] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.505784477] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.520771271] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.521083360] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 9305] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409432.763700913] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9302] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.073391159] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409433.073445442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.992241141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.526579 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.161177547] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.462914 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.246199523] [controller_manager]: Overrun might occur, Total time : 2426.716 us (Expected < 2000.000 us) --> Read time : 12.864 us, Update time : 2412.239 us, Write time : 1.613 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.006216951] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.502268 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409439.434062068] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.434115389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409440.434129650] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409440.434239659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409441.434104073] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409441.434166212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409442.434163204] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409442.434230552] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409443.434155498] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409443.434205553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409444.434259854] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409444.434315530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.434148025] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409445.434202869] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.783414939] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.334387877] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.434180812] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.434229184] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.892924859] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.892967801] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.099077143] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.099173547] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.434130389] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409447.434225630] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.434186935] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409448.434235297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.595842484] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.614335620] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.165297978] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.433943105] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.433998400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.721228677] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.721281176] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.433999883] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409450.434038716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409451.434230884] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409451.434307500] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409452.434199919] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409452.434253361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409453.433989726] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409453.434051784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409454.434154454] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409454.434230248] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409455.434100464] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409455.434166469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409456.434081412] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409456.434132860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409457.434134292] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409457.434197072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409458.433948272] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409458.434003066] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409459.434215665] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409459.434271941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409432.949648868] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409460.036698451] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:46513 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:46513 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.396550516] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.396607525] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.396623695] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.400874802] [moveit_1090737569.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.403194570] [moveit_1090737569.moveit.ros.rdf_loader]: Loaded robot model in 0.00216529 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.403227893] [moveit_1090737569.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.403236991] [moveit_1090737569.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409442.415048715] [moveit_1090737569.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.417972099] [moveit_1090737569.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.479782616] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.480010600] [moveit_1090737569.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481120994] [moveit_1090737569.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481598101] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481612919] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481748312] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482148212] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482233524] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482733814] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482749013] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.483157780] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.483591203] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409442.483949273] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409442.483970414] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.973274624] [moveit_1090737569.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.974414961] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.975805263] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.975824369] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976189643] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976205373] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976239728] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976250439] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976261259] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976991126] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.979382871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.979395244] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.981620287] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.981636437] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.982358480] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.018995259] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.022934028] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.023103079] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.023132004] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.023812477] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:24] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Generated 1 grasp options for object Mesh1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  IK accepted: 1 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016513833] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016631015] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016644711] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016663096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409447.016731406] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.017212781] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.017309254] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.034331148] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.034818875] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.035063309] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096413186] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096470325] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096503067] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096511844] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096544025] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098332793] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098379933] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098406062] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098538494] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.099385574] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.099409519] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409448.600248882] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409448.606065885] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.757943972] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.757994779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758002173] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758011360] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409449.758072987] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758319315] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758379840] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.762259084] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.762469064] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.763025205] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.763156835] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409459.763303860] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004847 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.767335099] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.767395965] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:39] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409460.166867386] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409475.300799510] [moveit_1090737569.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:18 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_mesh_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py::test_pick_and_place_mesh_by_id[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... +======================== 1 failed, 1 warning in 48.46s ========================= + + +12:38:05.70 [INFO] Long running tasks: + 114.20s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests + 161.18s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 327.50s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:38:34.30 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests +12:38:34.30 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-fzT5zi +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py::test_pick_and_place_box_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:38:15 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-12-775176-DESKTOP-HG3Q5KV-8237 (testutils.py:33) + +2026-05-10 12:38:15 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [8241] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [8242] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [8243] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-4]: process started with pid [8244] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-5]: process started with pid [8245] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [robot_state_publisher-2] [INFO] [1778409373.068470101] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.076757668] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.078945967] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.081048862] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.081078107] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.081083438] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.081172412] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.249956242] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.252875759] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.253289074] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.253363005] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.261915600] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263190413] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263223836] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263247421] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263253162] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263309950] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263346038] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.331357815] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.583676046] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.583735639] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.586388345] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.598535029] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.599607823] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.599735295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.606987490] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.607730313] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.610729356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.611519948] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.611574221] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.616268330] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.628869685] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.629221808] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.630365596] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.640653570] [controller_manager]: Overrun might occur, Total time : 9127.132 us (Expected < 2000.000 us) --> Read time : 11.482 us, Update time : 9113.376 us (Switch time : 9069.974 us (Switch chained mode time : 0.691 us, perform mode change time : 10.730 us, Activation time : 9042.732 us, Deactivation time : 0.241 us)), Write time : 2.274 us (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.640717141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.267481 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.640688923] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.642593517] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.643445300] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.643514762] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.643853345] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.654957715] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.656440718] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.656805591] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.656868110] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.658458893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.659681947] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.662675284] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.663656148] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.663699851] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.664813106] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.678640194] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.678907211] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.681066889] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.682544553] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.683650093] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.686513760] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.687350324] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.687393847] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.687702233] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.700494575] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.700766100] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.704371837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.705663852] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.708642497] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.709601464] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.709636882] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.710089627] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.719856270] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.730778994] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731025467] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731133803] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731147779] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731165744] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.733165730] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.740540753] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.741810335] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.744662840] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409373.840375727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 8244] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.092950336] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.093002665] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.093401453] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.116561043] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117651330] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117857582] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117905323] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117925742] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.119249121] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.126820173] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.126865879] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.128118965] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.142724541] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.143058941] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.143915279] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.147026466] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.147087622] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.147818150] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.162508088] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.162920862] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.163553364] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.167047913] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.167068923] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.167692318] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.182669251] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.183004232] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.183532987] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.187261066] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.187292927] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.187606342] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.202561279] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.202916749] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.213354532] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.213422621] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.213996032] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.230750356] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.231101208] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.237313998] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.237349826] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.237678861] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.255124435] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.255464111] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.260298752] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.260333508] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.260973094] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.277101960] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.277469503] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 8245] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [robot_state_publisher-2] [INFO] [1778409375.566933184] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8242] (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409375.904389573] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409375.904439778] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.798204480] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.755280 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.912496947] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.047718 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.758215760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.766491 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.942173203] [controller_manager]: Overrun might occur, Total time : 2663.261 us (Expected < 2000.000 us) --> Read time : 14.187 us, Update time : 2647.420 us, Write time : 1.654 us (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409381.207973435] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409381.208037948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409382.208230550] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409382.208400292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409383.208152500] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409383.208196203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409384.208236134] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409384.208277423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409385.208235344] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409385.208283926] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409386.208114213] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409386.208160211] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.208203329] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.208251460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409388.208092156] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409388.208135619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409389.208138014] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409389.208184392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409390.208144281] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409390.208194577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409391.208143880] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409391.208195268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409392.208231832] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409392.208282568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409393.208393525] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409393.208446215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409394.208162331] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409394.208211264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409395.208510539] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.208564942] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.910220010] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.770891 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409396.208224124] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409396.208273097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.208321200] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.208392245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.208189670] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409398.208238914] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409399.208266438] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409399.208315260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409400.208408999] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409400.208480936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409401.208239177] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.208283662] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.864181712] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.732663 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409402.208164936] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409402.208221203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.049880968] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431969 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409403.208251296] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.208323042] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409404.208159046] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409404.208205745] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409405.208386440] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.208441595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409405.833708418] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.866272614] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.823535 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.904120714] [controller_manager]: Overrun might occur, Total time : 2612.533 us (Expected < 2000.000 us) --> Read time : 15.289 us, Update time : 2595.401 us, Write time : 1.843 us (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409406.208160095] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.208213286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.384648681] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.945869051] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409406.945927943] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.072700416] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.072771231] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.208100689] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.208166464] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409408.208430366] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409408.208520117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409409.208315094] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409409.208356342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.035571210] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.208238760] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409410.208276993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409411.208347904] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409411.208403339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409412.207998256] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409412.208044494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409413.208235624] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409413.208288886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409414.208186725] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409414.208241699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409415.208100284] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409415.208161811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409416.208188421] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409416.208238366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409417.208108093] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409417.208153960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409418.208174207] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409418.208224833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409419.208063056] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.208115084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.208175989] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409420.208220934] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409421.208082611] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409421.208130352] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409422.208059911] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409422.208112380] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409423.208094003] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409423.208140001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409424.208077510] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409424.208129348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409425.159857961] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409425.208084436] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409425.208116898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409425.710863333] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.208332450] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.208382816] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.283054049] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.283105497] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.357607274] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.908604516] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.208183836] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409427.208232157] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409427.485536232] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.485586036] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.572615446] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.572686180] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409428.208101992] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409428.208170673] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.070218200] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.768971 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.163844887] [controller_manager]: Overrun might occur, Total time : 2313.205 us (Expected < 2000.000 us) --> Read time : 14.788 us, Update time : 2296.493 us, Write time : 1.924 us (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409429.208279066] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.208327208] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.208135602] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.208179576] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.208169584] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409431.208222936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409432.208307680] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409432.208365340] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.179578786] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.208124280] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409433.208175217] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409434.208082455] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.208151797] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.992153414] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.704075 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409435.208102301] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409435.208153508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.160550956] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.101477 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409436.209810664] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.210233127] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.243112903] [controller_manager]: Overrun might occur, Total time : 3603.567 us (Expected < 2000.000 us) --> Read time : 16.331 us, Update time : 3585.573 us, Write time : 1.663 us (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409437.208327254] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409437.208384873] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409438.208194875] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409438.208243878] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.006218013] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.768644 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409439.208327329] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.208383777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409440.208181276] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409440.208244716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409441.208260799] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409441.208292339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409442.208153169] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409442.208199136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409443.208129406] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409443.208177828] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409444.208187906] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409444.208239504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.208072982] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409445.208124479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.208028274] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.208071116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.208307154] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409447.208366907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.208263683] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409448.208324499] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.208083957] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.208134082] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.208159408] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409450.208210505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409451.208045591] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409451.208098170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409452.208287028] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409452.208339187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409453.208046984] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409453.208115754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409454.208147755] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409454.208215954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409455.208113984] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409455.208164811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409456.208219539] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409456.208272219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409457.208183345] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409457.208232207] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409458.208065719] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409458.208136334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409459.208151737] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409459.208197524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409460.208171064] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409460.208214726] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409461.208113353] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409461.208161234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409462.208316210] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409462.208371424] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409463.208187786] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409463.208234154] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409463.381260925] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409463.932328201] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.208038700] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409464.208089156] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409464.573984284] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.574040200] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.616489132] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.616551380] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409465.063583848] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409465.207856519] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409465.207902386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409466.208269664] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409466.208318647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409467.208149742] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409467.208201169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409468.208095074] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409468.208144238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409469.208041162] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409469.208089283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409470.208109626] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409470.208159300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409471.208109239] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409471.208157300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409472.208077962] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409472.208127757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409473.208094167] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409473.208145274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409474.208088029] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409474.208142743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409475.208274322] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409475.208326692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409476.208252324] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409476.208303751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409477.208258901] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409477.208313976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409478.208233463] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409478.208286785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409478.452754276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.304856 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409478.466182907] [controller_manager]: Overrun might occur, Total time : 4667.613 us (Expected < 2000.000 us) --> Read time : 17.142 us, Update time : 4648.698 us, Write time : 1.773 us (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.208282072] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409479.208334572] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.208296085] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409480.208337614] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409481.208100506] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409481.208157784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409482.208324476] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409482.208375784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409483.208055281] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409483.208105046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409484.208597809] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409484.208637515] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409484.330204175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.754975 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409485.208322832] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.208379920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.470149161] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.699511 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.522270903] [controller_manager]: Overrun might occur, Total time : 6743.634 us (Expected < 2000.000 us) --> Read time : 44.725 us, Update time : 6697.156 us, Write time : 1.753 us (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409486.208255164] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409486.208306472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409487.208007464] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409487.208052730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409488.208230615] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.208285830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.298204230] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.755011 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409489.208210063] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409489.208262883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409490.208179532] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409490.208231560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409491.208174759] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409491.208213031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409492.208135455] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409492.208176182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409493.208062761] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409493.208102867] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409494.208277176] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409494.208331540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409495.208033988] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409495.208078382] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [INFO] [1778409375.780969008] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] [INFO] [1778409399.647001065] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:38:16 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:20 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] * Running on http://127.0.0.1:36055 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] * Running on http://172.30.43.138:36055 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.405448672] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.405503687] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.405521631] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.412114729] [moveit_2647972108.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.413770896] [moveit_2647972108.moveit.ros.rdf_loader]: Loaded robot model in 0.00151281 seconds (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.413808518] [moveit_2647972108.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.413828556] [moveit_2647972108.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [WARN] [1778409384.424866358] [moveit_2647972108.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.427456970] [moveit_2647972108.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.478803576] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.478919366] [moveit_2647972108.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.479729225] [moveit_2647972108.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480292780] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480309913] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480465543] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480880983] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480971996] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.481670888] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.481696908] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.482234088] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.482723729] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [WARN] [1778409384.482956271] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409384.482968073] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.527493633] [moveit_2647972108.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.528237797] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529510455] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529526215] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529821711] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529837721] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529864743] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529870443] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529888428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.531002975] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.533789761] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.533802846] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.536449044] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.536465936] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.537114729] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.568789650] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.573106892] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.573266205] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.573291824] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.574089895] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:26] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:27 INFO  Generated 12 grasp options for object Box1. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.677263032] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.677294392] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.679067761] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_cup_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.679088891] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.680610343] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.680629269] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.682502829] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'upper_arm_link' (type 'Robot link') and 'base_link_inertia' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.682546963] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409405.828984722] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409405.829038624] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:45 INFO  IK accepted: 4 grasp options. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:46 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952698584] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952818542] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952832118] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952850793] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [WARN] [1778409406.952940634] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.953372465] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.953467114] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.045556509] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.047100473] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.047485034] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:47 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071423802] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071452727] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071480068] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071490248] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071513001] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071926846] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071998503] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.072029251] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.072168035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.073002295] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.073021271] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409410.073882431] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409410.079847045] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:06 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298506491] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298583136] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298598606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298616901] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [WARN] [1778409426.298701461] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.299020367] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.299086382] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.351725268] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.354094790] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409426.355060265] [moveit_2647972108.moveit.ros.validate_solution]: Computed path is not valid. Invalid states at index locations: [ 18 ] out of 36. Explanations follow in command line. Contacts are published on /display_contacts (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.355139636] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.355148633] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409426.355214007] [moveit_2647972108.moveit.ros.validate_solution]: Completed listing of explanations for invalid states. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409426.355324908] [moveit_py]: PlanningResponseAdapter 'ValidateSolution' failed with error code INVALID_MOTION_PLAN (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502115930] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502172828] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502181494] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502191162] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [WARN] [1778409427.502255665] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502472667] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502526279] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.563656258] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.568405908] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.570404916] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571485134] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571507376] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571532554] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571540288] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571561198] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571900914] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571949136] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571975305] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.572147222] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.572991416] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.573003619] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409433.223285935] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409433.227866574] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:28 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591183202] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591257764] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591270167] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591283633] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [WARN] [1778409464.591371059] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591653606] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591703391] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.612487102] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.612603664] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.612775600] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613587122] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613612140] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613631426] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613639822] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613659800] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.615902728] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.615938987] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.615959616] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.616078472] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.616813157] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.616825080] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409465.067224286] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409465.071801473] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:00 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:15 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:15] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409496.020399484] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:31 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:31 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:31 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:38:33 [ ERROR] [INFO] [1778409511.164291488] [moveit_2647972108.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_____________________ test_pick_and_place_box_by_id[ur5e] ______________________ + +start_processes = Urls(ros_domain_id='19', robot_url='http://0.0.0.0:36055', storage_url='http://127.0.0.1:52557') + + @pytest.mark.timeout(400) + def test_pick_and_place_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0.5, 0.5, 0.5, 0)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id + rest.call( +src/python/arcor2_web/rest.py:273: in call + _handle_response(resp) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +resp = + + def _handle_response(resp: requests.Response) -> None: +  """Raises exception if there is something wrong with the response. +  +  :param resp: +  :return: +  """ +  + if resp.status_code < 400: + return +  + decoded_content = resp.content.decode() +  + # here we try to handle different cases + try: + cont = json.loads(decoded_content) + except json.JsonException: + # response contains invalid JSON + raise RestException(decoded_content) +  + if isinstance(cont, str): # just plain text + raise RestException(cont) + elif isinstance(cont, dict): # this could be WebApiError + try: +> raise WebApiError.from_dict(cont) +E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Box1. + +src/python/arcor2_web/rest.py:340: WebApiError +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-12-775176-DESKTOP-HG3Q5KV-8237 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [8241] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [8242] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [8243] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [8244] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [8245] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409373.068470101] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.076757668] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.078945967] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.081048862] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.081078107] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.081083438] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.081172412] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.249956242] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.252875759] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.253289074] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.253363005] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.261915600] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263190413] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263223836] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263247421] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263253162] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263309950] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263346038] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.331357815] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.583676046] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.583735639] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.586388345] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.598535029] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.599607823] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.599735295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.606987490] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.607730313] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.610729356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.611519948] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.611574221] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.616268330] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.628869685] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.629221808] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.630365596] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.640653570] [controller_manager]: Overrun might occur, Total time : 9127.132 us (Expected < 2000.000 us) --> Read time : 11.482 us, Update time : 9113.376 us (Switch time : 9069.974 us (Switch chained mode time : 0.691 us, perform mode change time : 10.730 us, Activation time : 9042.732 us, Deactivation time : 0.241 us)), Write time : 2.274 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.640717141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.267481 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.640688923] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.642593517] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.643445300] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.643514762] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.643853345] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.654957715] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.656440718] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.656805591] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.656868110] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.658458893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.659681947] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.662675284] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.663656148] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.663699851] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.664813106] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.678640194] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.678907211] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.681066889] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.682544553] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.683650093] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.686513760] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.687350324] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.687393847] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.687702233] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.700494575] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.700766100] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.704371837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.705663852] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.708642497] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.709601464] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.709636882] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.710089627] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.719856270] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.730778994] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731025467] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731133803] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731147779] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731165744] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.733165730] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.740540753] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.741810335] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.744662840] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409373.840375727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 8244] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.092950336] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.093002665] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.093401453] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.116561043] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117651330] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117857582] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117905323] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117925742] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.119249121] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.126820173] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.126865879] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.128118965] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.142724541] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.143058941] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.143915279] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.147026466] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.147087622] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.147818150] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.162508088] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.162920862] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.163553364] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.167047913] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.167068923] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.167692318] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.182669251] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.183004232] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.183532987] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.187261066] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.187292927] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.187606342] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.202561279] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.202916749] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.213354532] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.213422621] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.213996032] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.230750356] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.231101208] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.237313998] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.237349826] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.237678861] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.255124435] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.255464111] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.260298752] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.260333508] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.260973094] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.277101960] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.277469503] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 8245] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409375.566933184] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8242] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409375.904389573] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409375.904439778] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.798204480] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.755280 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.912496947] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.047718 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.758215760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.766491 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.942173203] [controller_manager]: Overrun might occur, Total time : 2663.261 us (Expected < 2000.000 us) --> Read time : 14.187 us, Update time : 2647.420 us, Write time : 1.654 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409381.207973435] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409381.208037948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409382.208230550] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409382.208400292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409383.208152500] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409383.208196203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409384.208236134] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409384.208277423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409385.208235344] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409385.208283926] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409386.208114213] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409386.208160211] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.208203329] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.208251460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409388.208092156] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409388.208135619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409389.208138014] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409389.208184392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409390.208144281] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409390.208194577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409391.208143880] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409391.208195268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409392.208231832] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409392.208282568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409393.208393525] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409393.208446215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409394.208162331] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409394.208211264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409395.208510539] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.208564942] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.910220010] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.770891 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409396.208224124] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409396.208273097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.208321200] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.208392245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.208189670] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409398.208238914] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409399.208266438] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409399.208315260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409400.208408999] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409400.208480936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409401.208239177] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.208283662] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.864181712] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.732663 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409402.208164936] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409402.208221203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.049880968] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431969 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409403.208251296] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.208323042] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409404.208159046] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409404.208205745] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409405.208386440] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.208441595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409405.833708418] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.866272614] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.823535 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.904120714] [controller_manager]: Overrun might occur, Total time : 2612.533 us (Expected < 2000.000 us) --> Read time : 15.289 us, Update time : 2595.401 us, Write time : 1.843 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409406.208160095] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.208213286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.384648681] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.945869051] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409406.945927943] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.072700416] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.072771231] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.208100689] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.208166464] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409408.208430366] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409408.208520117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409409.208315094] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409409.208356342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.035571210] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.208238760] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409410.208276993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409411.208347904] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409411.208403339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409412.207998256] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409412.208044494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409413.208235624] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409413.208288886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409414.208186725] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409414.208241699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409415.208100284] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409415.208161811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409416.208188421] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409416.208238366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409417.208108093] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409417.208153960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409418.208174207] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409418.208224833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409419.208063056] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.208115084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.208175989] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409420.208220934] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409421.208082611] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409421.208130352] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409422.208059911] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409422.208112380] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409423.208094003] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409423.208140001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409424.208077510] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409424.208129348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409425.159857961] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409425.208084436] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409425.208116898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409425.710863333] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.208332450] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.208382816] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.283054049] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.283105497] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.357607274] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.908604516] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.208183836] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409427.208232157] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409427.485536232] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.485586036] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.572615446] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.572686180] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409428.208101992] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409428.208170673] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.070218200] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.768971 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.163844887] [controller_manager]: Overrun might occur, Total time : 2313.205 us (Expected < 2000.000 us) --> Read time : 14.788 us, Update time : 2296.493 us, Write time : 1.924 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409429.208279066] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.208327208] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.208135602] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.208179576] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.208169584] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409431.208222936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409432.208307680] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409432.208365340] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.179578786] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.208124280] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409433.208175217] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409434.208082455] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.208151797] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.992153414] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.704075 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409435.208102301] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409435.208153508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.160550956] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.101477 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409436.209810664] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.210233127] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.243112903] [controller_manager]: Overrun might occur, Total time : 3603.567 us (Expected < 2000.000 us) --> Read time : 16.331 us, Update time : 3585.573 us, Write time : 1.663 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409437.208327254] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409437.208384873] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409438.208194875] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409438.208243878] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.006218013] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.768644 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409439.208327329] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.208383777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409440.208181276] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409440.208244716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409441.208260799] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409441.208292339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409442.208153169] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409442.208199136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409443.208129406] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409443.208177828] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409444.208187906] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409444.208239504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.208072982] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409445.208124479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.208028274] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.208071116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.208307154] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409447.208366907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.208263683] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409448.208324499] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.208083957] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.208134082] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.208159408] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409450.208210505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409451.208045591] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409451.208098170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409452.208287028] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409452.208339187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409453.208046984] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409453.208115754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409454.208147755] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409454.208215954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409455.208113984] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409455.208164811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409456.208219539] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409456.208272219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409457.208183345] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409457.208232207] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409458.208065719] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409458.208136334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409459.208151737] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409459.208197524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409460.208171064] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409460.208214726] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409461.208113353] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409461.208161234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409462.208316210] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409462.208371424] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409463.208187786] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409463.208234154] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409463.381260925] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409463.932328201] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.208038700] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409464.208089156] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409464.573984284] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.574040200] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.616489132] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.616551380] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409465.063583848] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409465.207856519] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409465.207902386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409466.208269664] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409466.208318647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409467.208149742] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409467.208201169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409468.208095074] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409468.208144238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409469.208041162] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409469.208089283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409470.208109626] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409470.208159300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409471.208109239] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409471.208157300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409472.208077962] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409472.208127757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409473.208094167] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409473.208145274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409474.208088029] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409474.208142743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409475.208274322] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409475.208326692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409476.208252324] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409476.208303751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409477.208258901] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409477.208313976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409478.208233463] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409478.208286785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409478.452754276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.304856 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409478.466182907] [controller_manager]: Overrun might occur, Total time : 4667.613 us (Expected < 2000.000 us) --> Read time : 17.142 us, Update time : 4648.698 us, Write time : 1.773 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.208282072] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409479.208334572] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.208296085] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409480.208337614] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409481.208100506] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409481.208157784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409482.208324476] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409482.208375784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409483.208055281] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409483.208105046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409484.208597809] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409484.208637515] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409484.330204175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.754975 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409485.208322832] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.208379920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.470149161] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.699511 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.522270903] [controller_manager]: Overrun might occur, Total time : 6743.634 us (Expected < 2000.000 us) --> Read time : 44.725 us, Update time : 6697.156 us, Write time : 1.753 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409486.208255164] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409486.208306472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409487.208007464] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409487.208052730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409488.208230615] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.208285830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.298204230] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.755011 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409489.208210063] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409489.208262883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409490.208179532] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409490.208231560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409491.208174759] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409491.208213031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409492.208135455] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409492.208176182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409493.208062761] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409493.208102867] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409494.208277176] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409494.208331540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409495.208033988] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409495.208078382] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.780969008] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409399.647001065] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:20 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:36055 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:36055 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.405448672] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.405503687] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.405521631] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.412114729] [moveit_2647972108.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.413770896] [moveit_2647972108.moveit.ros.rdf_loader]: Loaded robot model in 0.00151281 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.413808518] [moveit_2647972108.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.413828556] [moveit_2647972108.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409384.424866358] [moveit_2647972108.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.427456970] [moveit_2647972108.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.478803576] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.478919366] [moveit_2647972108.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.479729225] [moveit_2647972108.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480292780] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480309913] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480465543] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480880983] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480971996] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.481670888] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.481696908] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.482234088] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.482723729] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409384.482956271] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409384.482968073] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.527493633] [moveit_2647972108.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.528237797] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529510455] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529526215] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529821711] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529837721] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529864743] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529870443] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529888428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.531002975] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.533789761] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.533802846] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.536449044] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.536465936] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.537114729] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.568789650] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.573106892] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.573266205] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.573291824] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.574089895] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:26] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Generated 12 grasp options for object Box1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.677263032] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.677294392] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.679067761] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_cup_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.679088891] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.680610343] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.680629269] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.682502829] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'upper_arm_link' (type 'Robot link') and 'base_link_inertia' (type 'Robot link'), which constitutes a collision. Contact information is not stored. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.682546963] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409405.828984722] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409405.829038624] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  IK accepted: 4 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952698584] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952818542] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952832118] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952850793] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409406.952940634] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.953372465] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.953467114] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.045556509] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.047100473] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.047485034] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071423802] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071452727] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071480068] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071490248] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071513001] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071926846] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071998503] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.072029251] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.072168035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.073002295] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.073021271] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.073882431] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.079847045] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298506491] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298583136] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298598606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298616901] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409426.298701461] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.299020367] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.299086382] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.351725268] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.354094790] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409426.355060265] [moveit_2647972108.moveit.ros.validate_solution]: Computed path is not valid. Invalid states at index locations: [ 18 ] out of 36. Explanations follow in command line. Contacts are published on /display_contacts +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.355139636] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.355148633] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409426.355214007] [moveit_2647972108.moveit.ros.validate_solution]: Completed listing of explanations for invalid states. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409426.355324908] [moveit_py]: PlanningResponseAdapter 'ValidateSolution' failed with error code INVALID_MOTION_PLAN +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502115930] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502172828] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502181494] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502191162] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409427.502255665] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502472667] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502526279] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.563656258] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.568405908] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.570404916] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571485134] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571507376] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571532554] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571540288] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571561198] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571900914] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571949136] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571975305] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.572147222] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.572991416] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.573003619] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409433.223285935] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409433.227866574] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591183202] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591257764] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591270167] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591283633] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409464.591371059] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591653606] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591703391] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.612487102] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.612603664] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.612775600] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613587122] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613612140] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613631426] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613639822] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613659800] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.615902728] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.615938987] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.615959616] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.616078472] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.616813157] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.616825080] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409465.067224286] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409465.071801473] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:00 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:15] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409496.020399484] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:31 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:31 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:31 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409511.164291488] [moveit_2647972108.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(400) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_rotated_box.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py::test_pick_and_place_box_by_id[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... +=================== 1 failed, 1 warning in 141.78s (0:02:21) =================== + + +12:38:35.74 [INFO] Long running tasks: + 191.22s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests + 357.54s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:38:48.71 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests +12:38:48.71 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-EXxbd3 +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py::test_pick_and_place_sphere_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:37:42 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-35-25-644363-DESKTOP-HG3Q5KV-7301 (testutils.py:33) + +2026-05-10 12:37:42 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [7305] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [7306] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [7307] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-4]: process started with pid [7308] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-5]: process started with pid [7309] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [robot_state_publisher-2] [INFO] [1778409325.942210256] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.947799250] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.950736577] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.953197293] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.953231608] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.953242869] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409325.953350278] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.120336732] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.123018868] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.123324519] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.123385605] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.130833908] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132086789] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132119311] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132143697] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132148466] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132218880] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132264066] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.219184727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.471755962] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.471859349] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.475205731] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.501292831] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.502905389] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.503185982] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.503235807] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.503268659] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.509578615] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.516867314] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.516922709] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.518092171] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.531235480] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.531527279] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.532303705] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.534892644] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.534953159] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.535888250] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.549227422] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.549550246] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.550219006] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.553425880] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.553492536] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.554308937] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.568900458] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.569222675] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.569770275] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.572971619] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.573014150] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.577406760] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.590761463] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.591060816] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.601405402] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.601457040] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.601886996] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.615404792] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.615737239] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.621544348] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.621584614] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.622001422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.636973710] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.637285186] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.641542316] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.641599645] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.642291695] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.659198425] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.659560593] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409326.781022611] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 7309] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.033381443] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.033434163] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.034814501] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.049050331] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.050262339] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.050331069] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.055115496] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.055939295] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.058943580] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.059809093] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.059851494] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.060220620] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.074733632] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.075025111] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.076317170] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.086903782] [controller_manager]: Overrun might occur, Total time : 9174.573 us (Expected < 2000.000 us) --> Read time : 13.265 us, Update time : 9159.004 us (Switch time : 9112.685 us (Switch chained mode time : 0.381 us, perform mode change time : 10.590 us, Activation time : 9086.385 us, Deactivation time : 0.140 us)), Write time : 2.304 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.086970298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.369513 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.086920683] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.088776741] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.089966326] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.089995522] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.090479456] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.103155146] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.104523251] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.104833771] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.104891340] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.106364179] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.107781928] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.110607070] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.111831933] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.111868602] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.112987494] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.130674112] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.130986585] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.133232552] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.134426440] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.135838925] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.138782207] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.140021839] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.140057276] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.140286040] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.154638114] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.154913282] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.158551080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.159850925] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.163190305] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.164577321] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.164606987] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.164915623] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.178563660] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.195106183] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195644772] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195819314] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195839432] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195850523] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.197441006] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.204700484] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.205932341] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.209285311] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 7308] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [robot_state_publisher-2] [INFO] [1778409328.446955673] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7306] (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409328.739644512] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409328.739702071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.678208112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.607607 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.853933576] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.333152 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.644270826] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.670582 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409335.168492845] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.168542008] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409336.168772539] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409336.168826491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409337.168743000] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409337.168798535] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409338.168720250] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409338.168795212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.109685823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.085299 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409339.168899071] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.168946251] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409340.168841029] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409340.168888549] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409341.168868701] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409341.168944595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.125686004] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.168751679] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.168832081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.676798513] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.168854544] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409343.168909579] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409343.259686463] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.259741808] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.654803816] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.654870663] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409344.168600452] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409344.168650137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.125736596] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.168692507] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409345.168736170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.331307588] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409345.882331048] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409346.168565487] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409346.168623286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409346.461173069] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409346.461215430] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409347.168623127] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409347.168675997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409348.168598029] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409348.168652983] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409349.168618695] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409349.168677768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409350.168572337] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409350.168640396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409351.168651363] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409351.168701368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409352.168680082] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409352.168736830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409353.168612348] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409353.168682622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409354.168627087] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409354.168701629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409355.168722153] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409355.168785623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409356.168614422] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409356.168682111] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409356.589691529] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409357.140678175] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409357.168566571] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409357.168615704] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409357.837277912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409357.837336223] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.168591348] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409358.168642565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.528516719] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.528591521] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.731702722] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.788355279] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409359.168687876] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409359.168743141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409359.339379391] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409359.959512833] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409359.959563079] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409360.168877187] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409360.168927893] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409361.168837946] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409361.168890947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409361.618218898] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.618494 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409361.714251653] [controller_manager]: Overrun might occur, Total time : 2590.437 us (Expected < 2000.000 us) --> Read time : 13.196 us, Update time : 2575.488 us, Write time : 1.753 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.168603671] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409362.168652564] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.168658992] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409363.168731670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409364.168563690] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409364.168614857] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409365.168670830] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409365.168725093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409366.168697603] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409366.168775061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409367.168768120] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.168819247] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.478199889] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.599615 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.504896572] [controller_manager]: Overrun might occur, Total time : 3234.988 us (Expected < 2000.000 us) --> Read time : 13.957 us, Update time : 3219.458 us, Write time : 1.573 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409368.168910775] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.168961120] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.726165594] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.564969 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.768172530] [controller_manager]: Overrun might occur, Total time : 2510.060 us (Expected < 2000.000 us) --> Read time : 13.867 us, Update time : 2494.610 us, Write time : 1.583 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409369.169061715] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409369.169118292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409370.018611113] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409370.168804323] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409370.168855200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409370.569586880] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.126530371] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.126580216] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.150843629] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.150901199] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.168786939] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.168861501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.520114881] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.514126 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.551684010] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.910258652] [controller_manager]: Overrun might occur, Total time : 2469.109 us (Expected < 2000.000 us) --> Read time : 666.808 us, Update time : 1800.157 us, Write time : 2.144 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409372.168876708] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409372.168932374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.168777523] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.168829060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.168775783] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409374.168819696] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409375.168705212] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409375.168763553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409376.168666972] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409376.168718791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409377.168645199] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.168695625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.798227590] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.627356 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409378.168606515] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.168658534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.912603861] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.003546 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.932849665] [controller_manager]: Overrun might occur, Total time : 3190.701 us (Expected < 2000.000 us) --> Read time : 13.475 us, Update time : 3175.673 us, Write time : 1.553 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409379.168561076] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409379.168616782] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409380.168611025] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.168664727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.758219147] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.618923 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.906390507] [controller_manager]: Overrun might occur, Total time : 2515.440 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2498.698 us, Write time : 1.623 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409381.168692727] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409381.168745437] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409382.168822073] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409382.168895964] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409383.168618988] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409383.168672269] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409384.168544386] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409384.168596786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409385.168642101] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409385.168699821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409386.168571648] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409386.168619880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409386.679756751] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.168627614] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.168678641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.230751648] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.803747322] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.803810622] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.926421709] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.926474179] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409388.037711899] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409388.168609043] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409388.168654981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409389.168650141] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409389.168705757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409390.168666223] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409390.168716829] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409391.168646240] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409391.168698810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409392.168828582] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409392.168884198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409393.168634648] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409393.168679894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409394.168718513] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409394.168769920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409395.168949629] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.169004944] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.909941197] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.340782 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.919957824] [controller_manager]: Overrun might occur, Total time : 2150.812 us (Expected < 2000.000 us) --> Read time : 17.423 us, Update time : 2131.726 us, Write time : 1.663 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409396.168922391] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409396.168985060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.168768905] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.168801838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.168825594] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409398.168885929] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409399.168617544] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409399.168670605] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409400.168685664] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409400.168740388] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409401.168576729] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.168627035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.864145748] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.545294 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.889363419] [controller_manager]: Overrun might occur, Total time : 3696.935 us (Expected < 2000.000 us) --> Read time : 15.209 us, Update time : 3680.103 us, Write time : 1.623 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409402.168785782] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409402.168838933] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.042217666] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.617312 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409403.168754002] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.168806141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409404.168930458] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409404.168988258] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409405.168656819] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.168713637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.883741647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141243 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409406.168748224] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.168792007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.168758304] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.168808890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409408.169015584] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409408.169073825] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409409.168998457] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409409.169058661] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.169063127] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409410.169117911] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409411.168889058] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409411.168949953] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409412.168682707] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409412.168735277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409413.168686195] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409413.168734176] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409414.168707079] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409414.168761112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409415.168682151] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409415.168731655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409416.168723509] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409416.168773504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409417.168728431] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409417.168788786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409418.168662024] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409418.168712651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409418.692097599] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409419.168590863] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.168647822] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.243053154] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.974630912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409419.974689082] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.132457961] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.132524468] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.168688443] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409420.168754629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.323711649] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409421.168665855] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409421.168718786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409422.168693455] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409422.168744622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409423.168757620] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409423.168808457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409424.168620052] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409424.168671309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409425.168679847] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409425.168725043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.168943416] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.168999473] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.168871052] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409427.168923962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409428.168735627] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409428.168787376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.070234811] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.634557 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.094189689] [controller_manager]: Overrun might occur, Total time : 4528.395 us (Expected < 2000.000 us) --> Read time : 13.746 us, Update time : 4512.875 us, Write time : 1.774 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409429.171402632] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.171751965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.168703576] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.168756106] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.169039363] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409431.169092334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409432.168863658] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409432.168908713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.168529702] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409433.168572122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409434.168659186] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.168714611] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.992257803] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.657528 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409435.090833216] [controller_manager]: Overrun might occur, Total time : 3161.207 us (Expected < 2000.000 us) --> Read time : 14.017 us, Update time : 3144.875 us, Write time : 2.315 us (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409435.168741616] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409435.168792142] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.162010756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.410522 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409436.170519778] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.172836610] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409437.168799060] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409437.168869524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409438.168667047] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409438.168718144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.008134935] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.534681 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409439.170499011] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.170532555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409440.168736591] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409440.168785474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409441.168805806] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409441.168850361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409442.168579248] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409442.168651325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409443.168638419] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409443.168682993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409444.168792460] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409444.168846292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.168762572] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409445.168820162] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.168688079] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.168757140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.168780098] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409447.168834010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.168510339] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409448.168563170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.168619830] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.168666699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.168730228] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409450.168786024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.461008411] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409451.012029797] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [1778409328.626331243] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] [INFO] [1778409365.249856828] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:37:42 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] * Running on http://127.0.0.1:55479 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] * Running on http://172.30.43.138:55479 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:38 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.947805416] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.947870740] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.947886250] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.954234687] [moveit_2862854068.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.956083179] [moveit_2862854068.moveit.ros.rdf_loader]: Loaded robot model in 0.00169881 seconds (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.956119037] [moveit_2862854068.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.956142101] [moveit_2862854068.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409338.969190801] [moveit_2862854068.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.972569200] [moveit_2862854068.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.028067814] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.028206107] [moveit_2862854068.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029004604] [moveit_2862854068.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029469507] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029483203] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029666977] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030101002] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030209608] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030926316] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030940914] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.031468777] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.031991991] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409339.032301409] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409339.032320736] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.086094973] [moveit_2862854068.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.086999963] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088387885] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088402984] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088720813] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088733968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088754166] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088757783] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088771980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.089527726] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.092121140] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.092135236] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.094831213] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.094848195] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.095581669] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.127577956] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.131480913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.131638001] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.131662277] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.132543862] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:40] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  Generated 20 grasp options for object Sphere1. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  IK accepted: 20 grasp options. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634391847] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634487779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634497508] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634513448] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409343.634582048] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634983040] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.635071508] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.649743456] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.650238176] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.650473904] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653099818] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653130857] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653160413] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653169821] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653188156] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654084109] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654116651] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654150005] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654273108] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.655061171] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.655079866] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409345.155802840] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409345.161998774] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:46 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572016536] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572069947] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572077562] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572086438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409346.572152504] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572399634] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572442024] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.576006642] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.576669040] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.577185752] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.577275187] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409356.577392325] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004906 seconds (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.586633244] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.586694701] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468429923] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468494736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468537698] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468553548] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409358.468680920] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.469034803] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.469089697] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.481595403] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.481675145] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.481798158] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527465432] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527497793] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527516499] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527523913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527544612] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528028391] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528070712] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528098304] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528217441] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528874800] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528887825] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.779388726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.783990746] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:00 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004290652] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004355265] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004367528] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004385041] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409360.004467758] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004754443] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004809999] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.008203543] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.008542507] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.012544340] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.013201114] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409370.013338494] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.008463 seconds (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.016164624] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.016223867] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129566204] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129617511] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129626218] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129636527] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409371.129710939] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129946907] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129996291] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.147975500] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148105236] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148245793] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148915560] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148946670] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148966267] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148973751] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148992897] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150020636] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150072485] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150092112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150235985] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.151109765] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.151124864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.601727028] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.607956204] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843686136] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843762842] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843775446] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843788110] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409387.843891847] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.844153975] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.844202167] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.858503170] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.858580477] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.858662933] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924176112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924204516] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924221548] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924229974] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924248880] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926018613] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926048620] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926066424] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926198525] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926765428] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926777140] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409388.077129527] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409388.081924032] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:43 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081911888] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081972363] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081981300] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081991981] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409420.082070380] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.082340934] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.082425179] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.094868617] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.094936957] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.095017520] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130348809] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130375690] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130392973] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130400297] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130430554] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132017229] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132057475] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132083144] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132201449] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132724573] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132738399] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.333148871] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.337930997] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:15 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:30 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666748406] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666823369] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666836894] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666851522] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [WARN] [1778409451.666934129] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.667204322] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.667256652] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.680041173] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.680161080] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.680298401] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Executing plan (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681315515] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681336776] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681350342] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681357034] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681371913] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682027677] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682058265] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682076460] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682211777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682914292] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682946714] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.833686322] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.839946267] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409462.270819172] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self.run() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self.spin_once() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] return next(self._cb_iter) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:46 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:29 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:29 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] conn.send({"status": "ok", "result": result}) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self._send_bytes(_ForkingPickler.dumps(obj)) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self._send(header + buf) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] n = write(self._handle, buf) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] BrokenPipeError: [Errno 32] Broken pipe (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:44 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:44 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:44 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:38:48 [ ERROR] [INFO] [1778409525.070796341] [moveit_2862854068.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +____________________ test_pick_and_place_sphere_by_id[ur5e] ____________________ + +self = +conn = +method = 'PUT', url = '/graspable/pick_up_object_by_id' +body = b'{"objectId":"Sphere1","effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' +headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '111'} +retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) +timeout = Timeout(connect=3.05, read=120, total=None), chunked = False +response_conn = +preload_content = False, decode_content = False, enforce_content_length = True + + def _make_request( + self, + conn: BaseHTTPConnection, + method: str, + url: str, + body: _TYPE_BODY | None = None, + headers: typing.Mapping[str, str] | None = None, + retries: Retry | None = None, + timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, + chunked: bool = False, + response_conn: BaseHTTPConnection | None = None, + preload_content: bool = True, + decode_content: bool = True, + enforce_content_length: bool = True, + ) -> BaseHTTPResponse: +  """ +  Perform a request on a given urllib connection object taken from our +  pool. +  +  :param conn: +  a connection from one of our connection pools +  +  :param method: +  HTTP request method (such as GET, POST, PUT, etc.) +  +  :param url: +  The URL to perform the request on. +  +  :param body: +  Data to send in the request body, either :class:`str`, :class:`bytes`, +  an iterable of :class:`str`/:class:`bytes`, or a file-like object. +  +  :param headers: +  Dictionary of custom headers to send, such as User-Agent, +  If-None-Match, etc. If None, pool headers are used. If provided, +  these headers completely replace any pool-specific headers. +  +  :param retries: +  Configure the number of retries to allow before raising a +  :class:`~urllib3.exceptions.MaxRetryError` exception. +  +  Pass ``None`` to retry until you receive a response. Pass a +  :class:`~urllib3.util.retry.Retry` object for fine-grained control +  over different types of retries. +  Pass an integer number to retry connection errors that many times, +  but no other types of errors. Pass zero to never retry. +  +  If ``False``, then retries are disabled and any exception is raised +  immediately. Also, instead of raising a MaxRetryError on redirects, +  the redirect response will be returned. +  +  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. +  +  :param timeout: +  If specified, overrides the default timeout for this one +  request. It may be a float (in seconds) or an instance of +  :class:`urllib3.util.Timeout`. +  +  :param chunked: +  If True, urllib3 will send the body using chunked transfer +  encoding. Otherwise, urllib3 will send the body using the standard +  content-length form. Defaults to False. +  +  :param response_conn: +  Set this to ``None`` if you will handle releasing the connection or +  set the connection to have the response release it. +  +  :param preload_content: +  If True, the response's body will be preloaded during construction. +  +  :param decode_content: +  If True, will attempt to decode the body based on the +  'content-encoding' header. +  +  :param enforce_content_length: +  Enforce content length checking. Body returned by server must match +  value of Content-Length header, if present. Otherwise, raise error. +  """ + self.num_requests += 1 +  + timeout_obj = self._get_timeout(timeout) + timeout_obj.start_connect() + conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) +  + try: + # Trigger any extra validation we need to do. + try: + self._validate_conn(conn) + except (SocketTimeout, BaseSSLError) as e: + self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) + raise +  + # _validate_conn() starts the connection to an HTTPS proxy + # so we need to wrap errors with 'ProxyError' here too. + except ( + OSError, + NewConnectionError, + TimeoutError, + BaseSSLError, + CertificateError, + SSLError, + ) as e: + new_e: Exception = e + if isinstance(e, (BaseSSLError, CertificateError)): + new_e = SSLError(e) + # If the connection didn't successfully connect to it's proxy + # then there + if isinstance( + new_e, (OSError, NewConnectionError, TimeoutError, SSLError) + ) and (conn and conn.proxy and not conn.has_connected_to_proxy): + new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) + raise new_e +  + # conn.request() calls http.client.*.request, not the method in + # urllib3.request. It also calls makefile (recv) on the socket. + try: + conn.request( + method, + url, + body=body, + headers=headers, + chunked=chunked, + preload_content=preload_content, + decode_content=decode_content, + enforce_content_length=enforce_content_length, + ) +  + # We are swallowing BrokenPipeError (errno.EPIPE) since the server is + # legitimately able to close the connection after sending a valid response. + # With this behaviour, the received response is still readable. + except BrokenPipeError: + pass + except OSError as e: + # MacOS/Linux + # EPROTOTYPE and ECONNRESET are needed on macOS + # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ + # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. + if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: + raise +  + # Reset the timeout for the recv() on the socket + read_timeout = timeout_obj.read_timeout +  + if not conn.is_closed: + # In Python 3 socket.py will catch EAGAIN and return None when you + # try and read into the file pointer created by http.client, which + # instead raises a BadStatusLine exception. Instead of catching + # the exception and assuming all BadStatusLine exceptions are read + # timeouts, check for a zero timeout before making the request. + if read_timeout == 0: + raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={read_timeout})" + ) + conn.timeout = read_timeout +  + # Receive the response from the server + try: +> response = conn.getresponse() + ^^^^^^^^^^^^^^^^^^ + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse + httplib_response = super().getresponse() + ^^^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:1428: in getresponse + response.begin() +/usr/lib/python3.12/http/client.py:331: in begin + version, status, reason = self._read_status() + ^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:292: in _read_status + line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +b = + + def readinto(self, b): +  """Read up to len(b) bytes into the writable buffer *b* and return +  the number of bytes read. If the socket is non-blocking and no bytes +  are available, None is returned. +  +  If *b* is non-empty, a 0 return value indicates that the connection +  was shutdown at the other end. +  """ + self._checkClosed() + self._checkReadable() + if self._timeout_occurred: + raise OSError("cannot read from timed out object") + while True: + try: +> return self._sock.recv_into(b) + ^^^^^^^^^^^^^^^^^^^^^^^ +E TimeoutError: timed out + +/usr/lib/python3.12/socket.py:707: TimeoutError + +The above exception was the direct cause of the following exception: + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: +> resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen + retries = retries.increment( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment + raise reraise(type(error), error, _stacktrace) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise + raise value +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen + response = self._make_request( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request + self._raise_timeout(err=e, url=url, timeout_value=read_timeout) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_id' +timeout_value = 120 + + def _raise_timeout( + self, + err: BaseSSLError | OSError | SocketTimeout, + url: str, + timeout_value: _TYPE_TIMEOUT | None, + ) -> None: +  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" +  + if isinstance(err, SocketTimeout): +> raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={timeout_value})" + ) from err +E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=55479): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError + +During handling of the above exception, another exception occurred: + +method = )> +url = 'http://0.0.0.0:55479/graspable/pick_up_object_by_id' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: +> resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + +src/python/arcor2_web/rest.py:259: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put + return request("put", url, data=data, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request + return session.request(method=method, url=url, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request + resp = self.send(prep, **send_kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send + r = adapter.send(request, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: + resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) +  + except (ProtocolError, OSError) as err: + raise ConnectionError(err, request=request) +  + except MaxRetryError as e: + if isinstance(e.reason, ConnectTimeoutError): + # TODO: Remove this in 3.0.0: see #2811 + if not isinstance(e.reason, NewConnectionError): + raise ConnectTimeout(e, request=request) +  + if isinstance(e.reason, ResponseError): + raise RetryError(e, request=request) +  + if isinstance(e.reason, _ProxyError): + raise ProxyError(e, request=request) +  + if isinstance(e.reason, _SSLError): + # This branch is for urllib3 v1.22 and later. + raise SSLError(e, request=request) +  + raise ConnectionError(e, request=request) +  + except ClosedPoolError as e: + raise ConnectionError(e, request=request) +  + except _ProxyError as e: + raise ProxyError(e) +  + except (_SSLError, _HTTPError) as e: + if isinstance(e, _SSLError): + # This branch is for urllib3 versions earlier than v1.22 + raise SSLError(e, request=request) + elif isinstance(e, ReadTimeoutError): +> raise ReadTimeout(e, request=request) +E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=55479): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout + +The above exception was the direct cause of the following exception: + +start_processes = Urls(ros_domain_id='34', robot_url='http://0.0.0.0:55479', storage_url='http://127.0.0.1:36283') + + @pytest.mark.timeout(321) + def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Sphere("Sphere1", 0.1) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id + rest.call( +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +method = )> +url = 'http://0.0.0.0:55479/graspable/pick_up_object_by_id' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: + resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + except requests.exceptions.RequestException as e: + logger.debug("Request failed.", exc_info=True) + # TODO would be good to provide more meaningful message but the original one could be very very long +> raise RestException("Catastrophic system error.") from e +E arcor2_web.rest.RestException: Catastrophic system error. + +src/python/arcor2_web/rest.py:269: RestException +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-35-25-644363-DESKTOP-HG3Q5KV-7301 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [7305] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [7306] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [7307] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [7308] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [7309] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409325.942210256] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.947799250] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.950736577] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.953197293] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.953231608] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.953242869] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409325.953350278] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.120336732] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.123018868] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.123324519] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.123385605] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.130833908] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132086789] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132119311] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132143697] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132148466] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132218880] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132264066] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.219184727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.471755962] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.471859349] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.475205731] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.501292831] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.502905389] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.503185982] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.503235807] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.503268659] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.509578615] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.516867314] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.516922709] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.518092171] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.531235480] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.531527279] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.532303705] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.534892644] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.534953159] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.535888250] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.549227422] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.549550246] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.550219006] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.553425880] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.553492536] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.554308937] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.568900458] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.569222675] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.569770275] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.572971619] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.573014150] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.577406760] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.590761463] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.591060816] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.601405402] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.601457040] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.601886996] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.615404792] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.615737239] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.621544348] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.621584614] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.622001422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.636973710] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.637285186] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.641542316] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.641599645] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.642291695] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.659198425] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.659560593] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409326.781022611] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 7309] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.033381443] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.033434163] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.034814501] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.049050331] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.050262339] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.050331069] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.055115496] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.055939295] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.058943580] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.059809093] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.059851494] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.060220620] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.074733632] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.075025111] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.076317170] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.086903782] [controller_manager]: Overrun might occur, Total time : 9174.573 us (Expected < 2000.000 us) --> Read time : 13.265 us, Update time : 9159.004 us (Switch time : 9112.685 us (Switch chained mode time : 0.381 us, perform mode change time : 10.590 us, Activation time : 9086.385 us, Deactivation time : 0.140 us)), Write time : 2.304 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.086970298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.369513 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.086920683] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.088776741] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.089966326] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.089995522] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.090479456] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.103155146] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.104523251] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.104833771] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.104891340] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.106364179] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.107781928] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.110607070] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.111831933] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.111868602] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.112987494] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.130674112] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.130986585] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.133232552] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.134426440] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.135838925] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.138782207] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.140021839] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.140057276] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.140286040] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.154638114] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.154913282] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.158551080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.159850925] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.163190305] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.164577321] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.164606987] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.164915623] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.178563660] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.195106183] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195644772] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195819314] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195839432] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195850523] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.197441006] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.204700484] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.205932341] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.209285311] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 7308] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409328.446955673] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7306] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409328.739644512] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409328.739702071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.678208112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.607607 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.853933576] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.333152 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.644270826] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.670582 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409335.168492845] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.168542008] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409336.168772539] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409336.168826491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409337.168743000] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409337.168798535] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409338.168720250] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409338.168795212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.109685823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.085299 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409339.168899071] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.168946251] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409340.168841029] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409340.168888549] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409341.168868701] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409341.168944595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.125686004] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.168751679] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.168832081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.676798513] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.168854544] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409343.168909579] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409343.259686463] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.259741808] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.654803816] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.654870663] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409344.168600452] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409344.168650137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.125736596] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.168692507] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409345.168736170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.331307588] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409345.882331048] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409346.168565487] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409346.168623286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409346.461173069] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409346.461215430] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409347.168623127] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409347.168675997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409348.168598029] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409348.168652983] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409349.168618695] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409349.168677768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409350.168572337] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409350.168640396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409351.168651363] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409351.168701368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409352.168680082] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409352.168736830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409353.168612348] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409353.168682622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409354.168627087] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409354.168701629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409355.168722153] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409355.168785623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409356.168614422] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409356.168682111] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409356.589691529] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409357.140678175] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409357.168566571] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409357.168615704] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409357.837277912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409357.837336223] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.168591348] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409358.168642565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.528516719] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.528591521] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.731702722] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.788355279] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409359.168687876] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409359.168743141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409359.339379391] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409359.959512833] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409359.959563079] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409360.168877187] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409360.168927893] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409361.168837946] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409361.168890947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409361.618218898] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.618494 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409361.714251653] [controller_manager]: Overrun might occur, Total time : 2590.437 us (Expected < 2000.000 us) --> Read time : 13.196 us, Update time : 2575.488 us, Write time : 1.753 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.168603671] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409362.168652564] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.168658992] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409363.168731670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409364.168563690] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409364.168614857] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409365.168670830] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409365.168725093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409366.168697603] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409366.168775061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409367.168768120] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.168819247] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.478199889] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.599615 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.504896572] [controller_manager]: Overrun might occur, Total time : 3234.988 us (Expected < 2000.000 us) --> Read time : 13.957 us, Update time : 3219.458 us, Write time : 1.573 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409368.168910775] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.168961120] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.726165594] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.564969 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.768172530] [controller_manager]: Overrun might occur, Total time : 2510.060 us (Expected < 2000.000 us) --> Read time : 13.867 us, Update time : 2494.610 us, Write time : 1.583 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409369.169061715] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409369.169118292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409370.018611113] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409370.168804323] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409370.168855200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409370.569586880] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.126530371] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.126580216] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.150843629] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.150901199] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.168786939] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.168861501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.520114881] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.514126 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.551684010] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.910258652] [controller_manager]: Overrun might occur, Total time : 2469.109 us (Expected < 2000.000 us) --> Read time : 666.808 us, Update time : 1800.157 us, Write time : 2.144 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409372.168876708] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409372.168932374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.168777523] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.168829060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.168775783] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409374.168819696] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409375.168705212] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409375.168763553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409376.168666972] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409376.168718791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409377.168645199] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.168695625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.798227590] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.627356 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409378.168606515] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.168658534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.912603861] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.003546 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.932849665] [controller_manager]: Overrun might occur, Total time : 3190.701 us (Expected < 2000.000 us) --> Read time : 13.475 us, Update time : 3175.673 us, Write time : 1.553 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409379.168561076] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409379.168616782] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409380.168611025] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.168664727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.758219147] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.618923 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.906390507] [controller_manager]: Overrun might occur, Total time : 2515.440 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2498.698 us, Write time : 1.623 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409381.168692727] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409381.168745437] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409382.168822073] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409382.168895964] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409383.168618988] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409383.168672269] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409384.168544386] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409384.168596786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409385.168642101] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409385.168699821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409386.168571648] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409386.168619880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409386.679756751] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.168627614] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.168678641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.230751648] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.803747322] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.803810622] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.926421709] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.926474179] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409388.037711899] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409388.168609043] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409388.168654981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409389.168650141] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409389.168705757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409390.168666223] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409390.168716829] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409391.168646240] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409391.168698810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409392.168828582] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409392.168884198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409393.168634648] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409393.168679894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409394.168718513] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409394.168769920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409395.168949629] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.169004944] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.909941197] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.340782 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.919957824] [controller_manager]: Overrun might occur, Total time : 2150.812 us (Expected < 2000.000 us) --> Read time : 17.423 us, Update time : 2131.726 us, Write time : 1.663 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409396.168922391] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409396.168985060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.168768905] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.168801838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.168825594] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409398.168885929] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409399.168617544] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409399.168670605] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409400.168685664] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409400.168740388] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409401.168576729] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.168627035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.864145748] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.545294 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.889363419] [controller_manager]: Overrun might occur, Total time : 3696.935 us (Expected < 2000.000 us) --> Read time : 15.209 us, Update time : 3680.103 us, Write time : 1.623 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409402.168785782] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409402.168838933] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.042217666] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.617312 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409403.168754002] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.168806141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409404.168930458] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409404.168988258] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409405.168656819] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.168713637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.883741647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141243 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409406.168748224] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.168792007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.168758304] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.168808890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409408.169015584] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409408.169073825] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409409.168998457] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409409.169058661] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.169063127] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409410.169117911] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409411.168889058] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409411.168949953] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409412.168682707] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409412.168735277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409413.168686195] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409413.168734176] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409414.168707079] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409414.168761112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409415.168682151] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409415.168731655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409416.168723509] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409416.168773504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409417.168728431] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409417.168788786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409418.168662024] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409418.168712651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409418.692097599] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409419.168590863] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.168647822] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.243053154] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.974630912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409419.974689082] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.132457961] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.132524468] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.168688443] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409420.168754629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.323711649] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409421.168665855] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409421.168718786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409422.168693455] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409422.168744622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409423.168757620] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409423.168808457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409424.168620052] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409424.168671309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409425.168679847] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409425.168725043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.168943416] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.168999473] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.168871052] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409427.168923962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409428.168735627] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409428.168787376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.070234811] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.634557 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.094189689] [controller_manager]: Overrun might occur, Total time : 4528.395 us (Expected < 2000.000 us) --> Read time : 13.746 us, Update time : 4512.875 us, Write time : 1.774 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409429.171402632] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.171751965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.168703576] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.168756106] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.169039363] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409431.169092334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409432.168863658] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409432.168908713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.168529702] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409433.168572122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409434.168659186] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.168714611] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.992257803] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.657528 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409435.090833216] [controller_manager]: Overrun might occur, Total time : 3161.207 us (Expected < 2000.000 us) --> Read time : 14.017 us, Update time : 3144.875 us, Write time : 2.315 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409435.168741616] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409435.168792142] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.162010756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.410522 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409436.170519778] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.172836610] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409437.168799060] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409437.168869524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409438.168667047] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409438.168718144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.008134935] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.534681 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409439.170499011] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.170532555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409440.168736591] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409440.168785474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409441.168805806] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409441.168850361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409442.168579248] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409442.168651325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409443.168638419] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409443.168682993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409444.168792460] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409444.168846292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.168762572] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409445.168820162] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.168688079] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.168757140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.168780098] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409447.168834010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.168510339] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409448.168563170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.168619830] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.168666699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.168730228] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409450.168786024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.461008411] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409451.012029797] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409328.626331243] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409365.249856828] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:55479 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:55479 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.947805416] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.947870740] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.947886250] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.954234687] [moveit_2862854068.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.956083179] [moveit_2862854068.moveit.ros.rdf_loader]: Loaded robot model in 0.00169881 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.956119037] [moveit_2862854068.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.956142101] [moveit_2862854068.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409338.969190801] [moveit_2862854068.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.972569200] [moveit_2862854068.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.028067814] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.028206107] [moveit_2862854068.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029004604] [moveit_2862854068.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029469507] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029483203] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029666977] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030101002] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030209608] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030926316] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030940914] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.031468777] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.031991991] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409339.032301409] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409339.032320736] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.086094973] [moveit_2862854068.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.086999963] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088387885] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088402984] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088720813] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088733968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088754166] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088757783] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088771980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.089527726] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.092121140] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.092135236] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.094831213] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.094848195] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.095581669] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.127577956] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.131480913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.131638001] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.131662277] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.132543862] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:40] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Generated 20 grasp options for object Sphere1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  IK accepted: 20 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634391847] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634487779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634497508] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634513448] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409343.634582048] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634983040] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.635071508] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.649743456] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.650238176] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.650473904] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653099818] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653130857] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653160413] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653169821] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653188156] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654084109] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654116651] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654150005] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654273108] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.655061171] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.655079866] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409345.155802840] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409345.161998774] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572016536] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572069947] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572077562] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572086438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409346.572152504] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572399634] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572442024] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.576006642] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.576669040] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.577185752] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.577275187] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409356.577392325] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004906 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.586633244] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.586694701] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468429923] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468494736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468537698] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468553548] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409358.468680920] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.469034803] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.469089697] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.481595403] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.481675145] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.481798158] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527465432] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527497793] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527516499] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527523913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527544612] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528028391] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528070712] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528098304] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528217441] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528874800] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528887825] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.779388726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.783990746] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:00 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004290652] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004355265] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004367528] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004385041] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409360.004467758] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004754443] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004809999] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.008203543] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.008542507] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.012544340] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.013201114] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409370.013338494] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.008463 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.016164624] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.016223867] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129566204] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129617511] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129626218] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129636527] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409371.129710939] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129946907] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129996291] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.147975500] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148105236] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148245793] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148915560] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148946670] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148966267] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148973751] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148992897] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150020636] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150072485] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150092112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150235985] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.151109765] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.151124864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.601727028] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.607956204] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843686136] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843762842] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843775446] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843788110] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409387.843891847] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.844153975] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.844202167] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.858503170] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.858580477] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.858662933] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924176112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924204516] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924221548] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924229974] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924248880] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926018613] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926048620] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926066424] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926198525] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926765428] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926777140] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409388.077129527] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409388.081924032] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:43 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081911888] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081972363] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081981300] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081991981] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409420.082070380] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.082340934] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.082425179] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.094868617] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.094936957] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.095017520] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130348809] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130375690] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130392973] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130400297] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130430554] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132017229] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132057475] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132083144] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132201449] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132724573] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132738399] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.333148871] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.337930997] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666748406] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666823369] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666836894] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666851522] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409451.666934129] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.667204322] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.667256652] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.680041173] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.680161080] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.680298401] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681315515] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681336776] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681350342] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681357034] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681371913] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682027677] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682058265] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682076460] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682211777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682914292] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682946714] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.833686322] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.839946267] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409462.270819172] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:46 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:29 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:29 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 conn.send({"status": "ok", "result": result}) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send_bytes(_ForkingPickler.dumps(obj)) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send(header + buf) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 n = write(self._handle, buf) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 BrokenPipeError: [Errno 32] Broken pipe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409525.070796341] [moveit_2862854068.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_sphere_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py::test_pick_and_place_sphere_by_id[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. +=================== 1 failed, 1 warning in 203.25s (0:03:23) =================== + + +12:39:05.80 [INFO] Long running tasks: + 67.55s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 387.60s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:39:35.84 [INFO] Long running tasks: + 61.53s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 97.58s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 417.64s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:40:05.87 [INFO] Long running tasks: + 91.57s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 127.62s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 447.67s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:40:35.91 [INFO] Long running tasks: + 121.61s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 157.66s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 477.71s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:41:05.94 [INFO] Long running tasks: + 151.64s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests + 187.69s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests + 507.74s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:41:22.34 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests +12:41:22.35 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-z7me8l +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py::test_pick_and_place_cylinder_by_id[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:40:17 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-59-296349-DESKTOP-HG3Q5KV-9863 (testutils.py:33) + +2026-05-10 12:40:17 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [9867] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [9868] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [9869] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-4]: process started with pid [9870] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-5]: process started with pid [9871] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409479.562757922] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.566787349] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.569029579] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.571343711] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.571373017] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.571378487] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409479.571474700] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.739618903] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.742206677] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.742606897] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.742671760] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.750900670] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752253025] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752289614] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752316626] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752323439] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752384204] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752431624] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409479.820239011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.072447737] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.072511608] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.074832744] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.088952584] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.089990035] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.090098161] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.097361146] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.098030037] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.100941340] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.101667373] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.101729952] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.106129733] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.116806379] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.117040043] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.118619264] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409480.128639493] [controller_manager]: Overrun might occur, Total time : 8797.336 us (Expected < 2000.000 us) --> Read time : 13.145 us, Update time : 8781.867 us (Switch time : 8742.521 us (Switch chained mode time : 0.341 us, perform mode change time : 9.668 us, Activation time : 8718.015 us, Deactivation time : 0.080 us)), Write time : 2.324 us (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409480.128698636] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.953060 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.128653274] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.130703374] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.131489949] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.131554311] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.131918814] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.141420579] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.143138514] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.143394644] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.143440542] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.146654789] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.147927808] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.150866017] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.151786405] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.151831751] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.152824427] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.169192559] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.169484363] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.171629549] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.174711486] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.175947965] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.178890020] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.179830492] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.179864928] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.180223399] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.192674592] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.192912730] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.196650262] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.197966945] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.200839482] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.201757065] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.201790899] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.202119584] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.212776975] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.225066856] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225355962] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225475459] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225490938] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225509473] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.227369978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.234620430] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.236060901] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.239039090] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.326539066] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 9870] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.578991900] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.579053597] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.579475258] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.603219325] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604705028] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604810649] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604837300] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604850274] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.606033072] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.613206212] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.613243703] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.614633223] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.626779497] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.627046414] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.627858127] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.631371592] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.631407210] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.632135294] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.646880090] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.647212572] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.647893717] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.651252178] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.651288447] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.652135947] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.666977974] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.667301434] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.667803457] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.671499741] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.671556448] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.671954309] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.684727760] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.684979243] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.693070613] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.693122772] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.693444809] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.708621206] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.708857986] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.715519317] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.715564062] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.716014784] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.730746371] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.731051405] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.735537364] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.735553886] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.735882841] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.750995908] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.751342562] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 9871] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409482.107213394] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9868] (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409482.391966653] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409482.392025345] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409484.330301179] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.556265 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.470242198] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.497234 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.298220101] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.475137 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409488.720124456] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.720171405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409489.720395432] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409489.720442231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409490.720355201] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409490.720399616] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409491.720391041] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409491.720443581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409492.720307764] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409492.720361286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409493.720417390] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409493.720470421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409494.720511422] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409494.720566136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409495.720411205] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409495.720467472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409496.720371880] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409496.720424169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409497.720383220] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409497.720436300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409498.674596944] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409498.720397306] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409498.720441811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409499.225602069] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409499.720305331] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409499.720354715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409500.720492378] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409500.720539949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409500.871464013] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409500.871509008] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409501.132842191] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409501.132917364] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409501.720280895] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409501.720334096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409502.617879268] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409502.720366622] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409502.720414843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409503.720250154] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409503.720305148] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409504.720431021] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409504.720480034] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409505.720388949] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409505.720441329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409506.720341403] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409506.720394664] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409507.525071651] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409507.720283241] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409507.720334949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409508.076086947] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409508.658494613] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409508.658547634] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409508.720429463] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409508.720485659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409509.720302581] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409509.720354530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409510.720313552] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409510.720363727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409511.720412740] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409511.720471993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409512.720448517] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409512.720498883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409513.720342691] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409513.720393267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409514.501844758] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.099423 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409514.526177313] [controller_manager]: Overrun might occur, Total time : 2369.903 us (Expected < 2000.000 us) --> Read time : 14.117 us, Update time : 2354.063 us, Write time : 1.723 us (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409514.720425403] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409514.720477612] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.720505274] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409515.720553105] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.720347835] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409516.720392149] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409517.720240542] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409517.720274887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409518.720389133] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409518.720441262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409518.822012840] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409519.373082213] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409519.720340350] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409519.720391457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.021969391] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.022028453] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.357891419] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.145292 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.410651269] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.410681216] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.720342529] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.720397002] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.741886530] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409521.520304427] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.559242 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409521.720464786] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409521.720521313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409522.720424134] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409522.720479830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409523.720361755] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409523.720406791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.334114784] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.369799 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409524.720339658] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.720393711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409525.720322599] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409525.720379096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409526.720479646] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409526.720534691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409527.720348135] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409527.720387489] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409528.720433335] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409528.720488861] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409529.720372399] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409529.720424648] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.720277497] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409530.720330728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409531.720278468] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409531.720329635] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.720303383] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409532.720359540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409533.720236245] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409533.720293434] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409534.720338738] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409534.720392581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409535.720269335] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409535.720342194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409536.720276228] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409536.720333997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409536.834883830] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409537.385860176] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409537.720255351] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409537.720316136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409538.213665969] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.213725212] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.248816855] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.248876098] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.720322943] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409538.720382056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409539.720357151] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409539.720418768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409540.187890606] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409540.720329118] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409540.720393921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409541.720310509] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409541.720365083] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409542.720325347] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409542.720383498] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409543.720406529] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409543.720466132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409544.720480870] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.720535163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.720548589] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409545.720601069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409546.720326829] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409546.720387053] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409547.720292670] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409547.720334259] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409548.720419115] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.720487796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409549.720436570] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409549.720509338] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409550.720270536] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409550.720332043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409551.720318204] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409551.720380232] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409552.720342944] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409552.720399651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409553.720248767] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409553.720301026] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409554.720377705] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409554.720432589] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409555.720358475] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409555.720417588] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409556.720299325] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409556.720361503] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409557.720377446] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409557.720428724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409558.720324087] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409558.720389412] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409559.720361902] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409559.720416155] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409560.720216905] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409560.720271037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.720310064] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409561.720365198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409562.720468849] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409562.720521799] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409563.720356843] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409563.720410144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409564.720274650] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409564.720328141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409565.720282908] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409565.720339806] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409566.720407301] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409566.720460472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409567.720415334] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409567.720467924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409568.720337780] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409568.720390830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409569.720269051] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409569.720322262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409570.354004687] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409570.720336967] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409570.720388385] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409570.904947122] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409571.705004811] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409571.705059645] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409571.720342049] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409571.720391613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.200788182] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.200835632] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.299887588] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.720415390] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409572.720468692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409573.720280886] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409573.720345619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409574.720313601] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409574.720353357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409575.720295788] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409575.720341264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409576.720392283] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409576.720452998] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409577.720315495] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409577.720355631] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.720288790] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409578.720343163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409579.720259421] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409579.720315347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409580.720300967] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409580.720353657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409581.720524012] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409581.720593283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409582.720241878] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409582.720302624] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409583.720245808] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409583.720283119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409584.720284460] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409584.720350836] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409585.720699541] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409585.720749566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409586.720348968] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409586.720410415] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409587.720417664] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409587.720477027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409588.720196842] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409588.720247969] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409589.720368133] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409589.720409532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409590.720279357] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409590.720338490] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409591.720207477] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409591.720264565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.720470829] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409592.720524421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409593.720557875] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409593.720617368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409594.720483105] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409594.720539582] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409595.720454843] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409595.720508735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409596.720546614] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409596.720599093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409597.720530134] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409597.720592502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409598.720311891] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409598.720367126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409599.720281896] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409599.720343343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409600.720468456] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409600.720503452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409601.720290564] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409601.720350638] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409602.460351145] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409602.720270151] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409602.720330957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.011341711] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.587578600] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.587632111] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.720419041] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.720476110] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.866640870] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.866702627] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409604.035896384] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409604.720176040] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409604.720228119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409605.165782039] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.036464 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409605.720306450] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409605.720366985] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409606.720258617] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409606.720360099] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409607.720299035] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409607.720352757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409608.720209235] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409608.720268378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409609.720195281] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409609.720252961] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.720276347] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409610.720333346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.720292480] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409611.720355510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409612.720312726] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409612.720371307] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409613.720448590] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409613.720528292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409614.720359002] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409614.720421381] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409615.720386977] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409615.720456890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409616.720238325] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409616.720292839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [1778409482.274302533] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] [INFO] [1778409518.151077238] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:40:17 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] * Running on http://127.0.0.1:46391 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] * Running on http://172.30.43.138:46391 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.897348835] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.897403238] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.897422816] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.903276087] [moveit_2981870913.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.904958734] [moveit_2981870913.moveit.ros.rdf_loader]: Loaded robot model in 0.001554 seconds (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.904984883] [moveit_2981870913.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.904993530] [moveit_2981870913.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409490.915268689] [moveit_2981870913.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.917684139] [moveit_2981870913.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.968975770] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.969101980] [moveit_2981870913.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970073354] [moveit_2981870913.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970635803] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970652144] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970778704] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971169737] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971238047] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971977833] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971992341] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.972503121] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.972974377] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409490.973210576] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409490.973221947] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.018992918] [moveit_2981870913.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.019647231] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.020733565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.020745698] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021036080] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021049736] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021076066] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021084622] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021092768] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021656699] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.023827905] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.023842713] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.025375871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.025392673] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.026338791] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.060652420] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.064083168] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.064241038] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.064271276] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.065021531] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:16] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Generated 20 grasp options for object Cyl1. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  IK accepted: 20 grasp options. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112724876] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112858089] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112887144] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112918914] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409501.112995971] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.113459802] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.113556105] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.129225328] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.129690542] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.129903537] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131176746] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131196554] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131230368] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131244044] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131262349] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132154830] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132181731] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132208762] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132328851] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.133141901] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.133163392] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409502.633798922] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409502.638179635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:28 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723005743] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723051380] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723058614] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723068202] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409508.723136933] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723359185] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723465367] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.728265709] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.729542155] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.731513947] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.733111673] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409518.733255350] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.009719 seconds (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.752298446] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.752363479] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386466390] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386526123] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386535060] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386546432] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409520.386629740] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386830692] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386875327] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.403242471] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.403365846] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.403534386] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405519588] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405540918] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405558161] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405567098] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405586635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410153317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410187132] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410210456] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410326216] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.412025119] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.412035750] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.761397038] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.766119069] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:58 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219042785] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219111646] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219121475] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219132085] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [WARN] [1778409538.219201206] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219442986] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219486328] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.244343716] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.245151571] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.245736012] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:38:58 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246576824] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246602343] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246625316] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246633772] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246655494] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248162236] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248193065] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248210919] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248354091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.249128953] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.249141767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:38:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:38:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409540.199555801] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409540.204121502] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:15 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:30 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:31 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182807387] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182895745] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182909761] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182921854] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [WARN] [1778409572.182980426] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.183232274] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.183275216] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.196634030] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.196702079] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.196759829] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199676825] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199702755] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199720739] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199730788] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199751317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200247800] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200284240] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200309057] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200492185] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.201081339] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.201093933] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.301518928] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.306119871] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:33 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:33 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:34 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:34 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:47 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790751605] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790826097] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790836116] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790847287] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [WARN] [1778409603.790905768] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.791185991] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.791242408] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.804213575] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.804277416] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.804406882] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864159500] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864183025] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864197252] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864204626] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864221187] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866181662] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866214164] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866237728] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866354290] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866923035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866934767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409604.067436224] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409604.072103202] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:13 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:13 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409617.594499160] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:19 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:04 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:19 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:19 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:19 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:41:22 [ ERROR] [INFO] [1778409679.412950789] [moveit_2981870913.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +___________________ test_pick_and_place_cylinder_by_id[ur5e] ___________________ + +self = +conn = +method = 'PUT', url = '/graspable/pick_up_object_by_id' +body = b'{"objectId":"Cyl1","effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' +headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '108'} +retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) +timeout = Timeout(connect=3.05, read=120, total=None), chunked = False +response_conn = +preload_content = False, decode_content = False, enforce_content_length = True + + def _make_request( + self, + conn: BaseHTTPConnection, + method: str, + url: str, + body: _TYPE_BODY | None = None, + headers: typing.Mapping[str, str] | None = None, + retries: Retry | None = None, + timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, + chunked: bool = False, + response_conn: BaseHTTPConnection | None = None, + preload_content: bool = True, + decode_content: bool = True, + enforce_content_length: bool = True, + ) -> BaseHTTPResponse: +  """ +  Perform a request on a given urllib connection object taken from our +  pool. +  +  :param conn: +  a connection from one of our connection pools +  +  :param method: +  HTTP request method (such as GET, POST, PUT, etc.) +  +  :param url: +  The URL to perform the request on. +  +  :param body: +  Data to send in the request body, either :class:`str`, :class:`bytes`, +  an iterable of :class:`str`/:class:`bytes`, or a file-like object. +  +  :param headers: +  Dictionary of custom headers to send, such as User-Agent, +  If-None-Match, etc. If None, pool headers are used. If provided, +  these headers completely replace any pool-specific headers. +  +  :param retries: +  Configure the number of retries to allow before raising a +  :class:`~urllib3.exceptions.MaxRetryError` exception. +  +  Pass ``None`` to retry until you receive a response. Pass a +  :class:`~urllib3.util.retry.Retry` object for fine-grained control +  over different types of retries. +  Pass an integer number to retry connection errors that many times, +  but no other types of errors. Pass zero to never retry. +  +  If ``False``, then retries are disabled and any exception is raised +  immediately. Also, instead of raising a MaxRetryError on redirects, +  the redirect response will be returned. +  +  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. +  +  :param timeout: +  If specified, overrides the default timeout for this one +  request. It may be a float (in seconds) or an instance of +  :class:`urllib3.util.Timeout`. +  +  :param chunked: +  If True, urllib3 will send the body using chunked transfer +  encoding. Otherwise, urllib3 will send the body using the standard +  content-length form. Defaults to False. +  +  :param response_conn: +  Set this to ``None`` if you will handle releasing the connection or +  set the connection to have the response release it. +  +  :param preload_content: +  If True, the response's body will be preloaded during construction. +  +  :param decode_content: +  If True, will attempt to decode the body based on the +  'content-encoding' header. +  +  :param enforce_content_length: +  Enforce content length checking. Body returned by server must match +  value of Content-Length header, if present. Otherwise, raise error. +  """ + self.num_requests += 1 +  + timeout_obj = self._get_timeout(timeout) + timeout_obj.start_connect() + conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) +  + try: + # Trigger any extra validation we need to do. + try: + self._validate_conn(conn) + except (SocketTimeout, BaseSSLError) as e: + self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) + raise +  + # _validate_conn() starts the connection to an HTTPS proxy + # so we need to wrap errors with 'ProxyError' here too. + except ( + OSError, + NewConnectionError, + TimeoutError, + BaseSSLError, + CertificateError, + SSLError, + ) as e: + new_e: Exception = e + if isinstance(e, (BaseSSLError, CertificateError)): + new_e = SSLError(e) + # If the connection didn't successfully connect to it's proxy + # then there + if isinstance( + new_e, (OSError, NewConnectionError, TimeoutError, SSLError) + ) and (conn and conn.proxy and not conn.has_connected_to_proxy): + new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) + raise new_e +  + # conn.request() calls http.client.*.request, not the method in + # urllib3.request. It also calls makefile (recv) on the socket. + try: + conn.request( + method, + url, + body=body, + headers=headers, + chunked=chunked, + preload_content=preload_content, + decode_content=decode_content, + enforce_content_length=enforce_content_length, + ) +  + # We are swallowing BrokenPipeError (errno.EPIPE) since the server is + # legitimately able to close the connection after sending a valid response. + # With this behaviour, the received response is still readable. + except BrokenPipeError: + pass + except OSError as e: + # MacOS/Linux + # EPROTOTYPE and ECONNRESET are needed on macOS + # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ + # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. + if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: + raise +  + # Reset the timeout for the recv() on the socket + read_timeout = timeout_obj.read_timeout +  + if not conn.is_closed: + # In Python 3 socket.py will catch EAGAIN and return None when you + # try and read into the file pointer created by http.client, which + # instead raises a BadStatusLine exception. Instead of catching + # the exception and assuming all BadStatusLine exceptions are read + # timeouts, check for a zero timeout before making the request. + if read_timeout == 0: + raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={read_timeout})" + ) + conn.timeout = read_timeout +  + # Receive the response from the server + try: +> response = conn.getresponse() + ^^^^^^^^^^^^^^^^^^ + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse + httplib_response = super().getresponse() + ^^^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:1428: in getresponse + response.begin() +/usr/lib/python3.12/http/client.py:331: in begin + version, status, reason = self._read_status() + ^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:292: in _read_status + line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +b = + + def readinto(self, b): +  """Read up to len(b) bytes into the writable buffer *b* and return +  the number of bytes read. If the socket is non-blocking and no bytes +  are available, None is returned. +  +  If *b* is non-empty, a 0 return value indicates that the connection +  was shutdown at the other end. +  """ + self._checkClosed() + self._checkReadable() + if self._timeout_occurred: + raise OSError("cannot read from timed out object") + while True: + try: +> return self._sock.recv_into(b) + ^^^^^^^^^^^^^^^^^^^^^^^ +E TimeoutError: timed out + +/usr/lib/python3.12/socket.py:707: TimeoutError + +The above exception was the direct cause of the following exception: + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: +> resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen + retries = retries.increment( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment + raise reraise(type(error), error, _stacktrace) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise + raise value +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen + response = self._make_request( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request + self._raise_timeout(err=e, url=url, timeout_value=read_timeout) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_id' +timeout_value = 120 + + def _raise_timeout( + self, + err: BaseSSLError | OSError | SocketTimeout, + url: str, + timeout_value: _TYPE_TIMEOUT | None, + ) -> None: +  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" +  + if isinstance(err, SocketTimeout): +> raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={timeout_value})" + ) from err +E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=46391): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError + +During handling of the above exception, another exception occurred: + +method = )> +url = 'http://0.0.0.0:46391/graspable/pick_up_object_by_id' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: +> resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + +src/python/arcor2_web/rest.py:259: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put + return request("put", url, data=data, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request + return session.request(method=method, url=url, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request + resp = self.send(prep, **send_kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send + r = adapter.send(request, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: + resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) +  + except (ProtocolError, OSError) as err: + raise ConnectionError(err, request=request) +  + except MaxRetryError as e: + if isinstance(e.reason, ConnectTimeoutError): + # TODO: Remove this in 3.0.0: see #2811 + if not isinstance(e.reason, NewConnectionError): + raise ConnectTimeout(e, request=request) +  + if isinstance(e.reason, ResponseError): + raise RetryError(e, request=request) +  + if isinstance(e.reason, _ProxyError): + raise ProxyError(e, request=request) +  + if isinstance(e.reason, _SSLError): + # This branch is for urllib3 v1.22 and later. + raise SSLError(e, request=request) +  + raise ConnectionError(e, request=request) +  + except ClosedPoolError as e: + raise ConnectionError(e, request=request) +  + except _ProxyError as e: + raise ProxyError(e) +  + except (_SSLError, _HTTPError) as e: + if isinstance(e, _SSLError): + # This branch is for urllib3 versions earlier than v1.22 + raise SSLError(e, request=request) + elif isinstance(e, ReadTimeoutError): +> raise ReadTimeout(e, request=request) +E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=46391): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout + +The above exception was the direct cause of the following exception: + +start_processes = Urls(ros_domain_id='35', robot_url='http://0.0.0.0:46391', storage_url='http://127.0.0.1:52405') + + @pytest.mark.timeout(321) + def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Cylinder("Cyl1", 0.1, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id + rest.call( +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +method = )> +url = 'http://0.0.0.0:46391/graspable/pick_up_object_by_id' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: + resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + except requests.exceptions.RequestException as e: + logger.debug("Request failed.", exc_info=True) + # TODO would be good to provide more meaningful message but the original one could be very very long +> raise RestException("Catastrophic system error.") from e +E arcor2_web.rest.RestException: Catastrophic system error. + +src/python/arcor2_web/rest.py:269: RestException +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-59-296349-DESKTOP-HG3Q5KV-9863 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [9867] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [9868] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [9869] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [9870] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [9871] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409479.562757922] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.566787349] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.569029579] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.571343711] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.571373017] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.571378487] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409479.571474700] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.739618903] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.742206677] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.742606897] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.742671760] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.750900670] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752253025] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752289614] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752316626] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752323439] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752384204] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752431624] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409479.820239011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.072447737] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.072511608] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.074832744] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.088952584] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.089990035] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.090098161] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.097361146] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.098030037] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.100941340] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.101667373] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.101729952] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.106129733] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.116806379] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.117040043] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.118619264] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409480.128639493] [controller_manager]: Overrun might occur, Total time : 8797.336 us (Expected < 2000.000 us) --> Read time : 13.145 us, Update time : 8781.867 us (Switch time : 8742.521 us (Switch chained mode time : 0.341 us, perform mode change time : 9.668 us, Activation time : 8718.015 us, Deactivation time : 0.080 us)), Write time : 2.324 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409480.128698636] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.953060 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.128653274] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.130703374] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.131489949] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.131554311] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.131918814] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.141420579] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.143138514] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.143394644] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.143440542] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.146654789] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.147927808] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.150866017] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.151786405] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.151831751] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.152824427] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.169192559] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.169484363] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.171629549] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.174711486] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.175947965] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.178890020] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.179830492] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.179864928] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.180223399] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.192674592] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.192912730] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.196650262] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.197966945] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.200839482] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.201757065] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.201790899] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.202119584] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.212776975] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.225066856] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225355962] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225475459] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225490938] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225509473] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.227369978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.234620430] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.236060901] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.239039090] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.326539066] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 9870] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.578991900] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.579053597] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.579475258] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.603219325] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604705028] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604810649] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604837300] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604850274] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.606033072] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.613206212] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.613243703] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.614633223] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.626779497] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.627046414] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.627858127] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.631371592] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.631407210] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.632135294] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.646880090] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.647212572] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.647893717] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.651252178] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.651288447] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.652135947] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.666977974] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.667301434] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.667803457] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.671499741] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.671556448] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.671954309] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.684727760] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.684979243] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.693070613] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.693122772] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.693444809] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.708621206] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.708857986] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.715519317] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.715564062] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.716014784] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.730746371] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.731051405] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.735537364] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.735553886] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.735882841] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.750995908] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.751342562] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 9871] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409482.107213394] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9868] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409482.391966653] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409482.392025345] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409484.330301179] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.556265 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.470242198] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.497234 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.298220101] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.475137 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409488.720124456] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.720171405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409489.720395432] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409489.720442231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409490.720355201] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409490.720399616] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409491.720391041] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409491.720443581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409492.720307764] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409492.720361286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409493.720417390] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409493.720470421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409494.720511422] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409494.720566136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409495.720411205] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409495.720467472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409496.720371880] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409496.720424169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409497.720383220] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409497.720436300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409498.674596944] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409498.720397306] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409498.720441811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409499.225602069] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409499.720305331] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409499.720354715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409500.720492378] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409500.720539949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409500.871464013] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409500.871509008] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409501.132842191] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409501.132917364] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409501.720280895] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409501.720334096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409502.617879268] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409502.720366622] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409502.720414843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409503.720250154] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409503.720305148] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409504.720431021] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409504.720480034] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409505.720388949] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409505.720441329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409506.720341403] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409506.720394664] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409507.525071651] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409507.720283241] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409507.720334949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409508.076086947] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409508.658494613] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409508.658547634] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409508.720429463] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409508.720485659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409509.720302581] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409509.720354530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409510.720313552] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409510.720363727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409511.720412740] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409511.720471993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409512.720448517] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409512.720498883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409513.720342691] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409513.720393267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409514.501844758] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.099423 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409514.526177313] [controller_manager]: Overrun might occur, Total time : 2369.903 us (Expected < 2000.000 us) --> Read time : 14.117 us, Update time : 2354.063 us, Write time : 1.723 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409514.720425403] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409514.720477612] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.720505274] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409515.720553105] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.720347835] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409516.720392149] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409517.720240542] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409517.720274887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409518.720389133] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409518.720441262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409518.822012840] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409519.373082213] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409519.720340350] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409519.720391457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.021969391] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.022028453] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.357891419] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.145292 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.410651269] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.410681216] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.720342529] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.720397002] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.741886530] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409521.520304427] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.559242 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409521.720464786] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409521.720521313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409522.720424134] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409522.720479830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409523.720361755] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409523.720406791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.334114784] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.369799 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409524.720339658] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.720393711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409525.720322599] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409525.720379096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409526.720479646] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409526.720534691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409527.720348135] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409527.720387489] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409528.720433335] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409528.720488861] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409529.720372399] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409529.720424648] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.720277497] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409530.720330728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409531.720278468] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409531.720329635] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.720303383] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409532.720359540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409533.720236245] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409533.720293434] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409534.720338738] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409534.720392581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409535.720269335] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409535.720342194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409536.720276228] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409536.720333997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409536.834883830] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409537.385860176] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409537.720255351] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409537.720316136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409538.213665969] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.213725212] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.248816855] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.248876098] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.720322943] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409538.720382056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409539.720357151] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409539.720418768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409540.187890606] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409540.720329118] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409540.720393921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409541.720310509] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409541.720365083] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409542.720325347] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409542.720383498] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409543.720406529] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409543.720466132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409544.720480870] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.720535163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.720548589] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409545.720601069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409546.720326829] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409546.720387053] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409547.720292670] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409547.720334259] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409548.720419115] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.720487796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409549.720436570] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409549.720509338] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409550.720270536] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409550.720332043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409551.720318204] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409551.720380232] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409552.720342944] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409552.720399651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409553.720248767] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409553.720301026] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409554.720377705] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409554.720432589] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409555.720358475] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409555.720417588] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409556.720299325] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409556.720361503] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409557.720377446] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409557.720428724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409558.720324087] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409558.720389412] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409559.720361902] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409559.720416155] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409560.720216905] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409560.720271037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.720310064] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409561.720365198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409562.720468849] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409562.720521799] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409563.720356843] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409563.720410144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409564.720274650] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409564.720328141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409565.720282908] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409565.720339806] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409566.720407301] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409566.720460472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409567.720415334] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409567.720467924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409568.720337780] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409568.720390830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409569.720269051] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409569.720322262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409570.354004687] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409570.720336967] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409570.720388385] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409570.904947122] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409571.705004811] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409571.705059645] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409571.720342049] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409571.720391613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.200788182] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.200835632] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.299887588] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.720415390] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409572.720468692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409573.720280886] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409573.720345619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409574.720313601] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409574.720353357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409575.720295788] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409575.720341264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409576.720392283] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409576.720452998] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409577.720315495] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409577.720355631] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.720288790] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409578.720343163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409579.720259421] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409579.720315347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409580.720300967] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409580.720353657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409581.720524012] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409581.720593283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409582.720241878] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409582.720302624] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409583.720245808] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409583.720283119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409584.720284460] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409584.720350836] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409585.720699541] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409585.720749566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409586.720348968] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409586.720410415] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409587.720417664] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409587.720477027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409588.720196842] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409588.720247969] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409589.720368133] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409589.720409532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409590.720279357] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409590.720338490] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409591.720207477] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409591.720264565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.720470829] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409592.720524421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409593.720557875] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409593.720617368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409594.720483105] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409594.720539582] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409595.720454843] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409595.720508735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409596.720546614] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409596.720599093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409597.720530134] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409597.720592502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409598.720311891] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409598.720367126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409599.720281896] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409599.720343343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409600.720468456] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409600.720503452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409601.720290564] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409601.720350638] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409602.460351145] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409602.720270151] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409602.720330957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.011341711] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.587578600] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.587632111] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.720419041] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.720476110] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.866640870] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.866702627] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409604.035896384] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409604.720176040] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409604.720228119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409605.165782039] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.036464 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409605.720306450] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409605.720366985] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409606.720258617] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409606.720360099] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409607.720299035] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409607.720352757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409608.720209235] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409608.720268378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409609.720195281] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409609.720252961] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.720276347] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409610.720333346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.720292480] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409611.720355510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409612.720312726] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409612.720371307] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409613.720448590] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409613.720528292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409614.720359002] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409614.720421381] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409615.720386977] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409615.720456890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409616.720238325] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409616.720292839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409482.274302533] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409518.151077238] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:46391 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:46391 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.897348835] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.897403238] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.897422816] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.903276087] [moveit_2981870913.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.904958734] [moveit_2981870913.moveit.ros.rdf_loader]: Loaded robot model in 0.001554 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.904984883] [moveit_2981870913.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.904993530] [moveit_2981870913.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409490.915268689] [moveit_2981870913.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.917684139] [moveit_2981870913.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.968975770] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.969101980] [moveit_2981870913.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970073354] [moveit_2981870913.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970635803] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970652144] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970778704] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971169737] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971238047] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971977833] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971992341] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.972503121] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.972974377] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409490.973210576] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409490.973221947] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.018992918] [moveit_2981870913.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.019647231] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.020733565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.020745698] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021036080] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021049736] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021076066] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021084622] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021092768] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021656699] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.023827905] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.023842713] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.025375871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.025392673] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.026338791] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.060652420] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.064083168] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.064241038] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.064271276] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.065021531] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:16] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Generated 20 grasp options for object Cyl1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  IK accepted: 20 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112724876] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112858089] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112887144] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112918914] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409501.112995971] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.113459802] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.113556105] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.129225328] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.129690542] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.129903537] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131176746] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131196554] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131230368] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131244044] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131262349] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132154830] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132181731] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132208762] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132328851] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.133141901] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.133163392] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409502.633798922] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409502.638179635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:28 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723005743] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723051380] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723058614] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723068202] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409508.723136933] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723359185] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723465367] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.728265709] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.729542155] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.731513947] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.733111673] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409518.733255350] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.009719 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.752298446] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.752363479] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386466390] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386526123] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386535060] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386546432] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409520.386629740] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386830692] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386875327] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.403242471] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.403365846] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.403534386] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405519588] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405540918] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405558161] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405567098] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405586635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410153317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410187132] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410210456] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410326216] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.412025119] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.412035750] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.761397038] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.766119069] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219042785] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219111646] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219121475] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219132085] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409538.219201206] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219442986] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219486328] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.244343716] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.245151571] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.245736012] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246576824] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246602343] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246625316] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246633772] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246655494] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248162236] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248193065] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248210919] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248354091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.249128953] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.249141767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409540.199555801] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409540.204121502] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:15 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:30 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:31 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182807387] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182895745] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182909761] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182921854] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409572.182980426] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.183232274] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.183275216] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.196634030] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.196702079] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.196759829] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199676825] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199702755] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199720739] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199730788] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199751317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200247800] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200284240] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200309057] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200492185] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.201081339] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.201093933] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.301518928] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.306119871] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:33 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:33 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:34 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:34 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:47 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790751605] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790826097] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790836116] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790847287] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409603.790905768] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.791185991] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.791242408] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.804213575] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.804277416] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.804406882] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864159500] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864183025] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864197252] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864204626] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864221187] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866181662] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866214164] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866237728] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866354290] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866923035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866934767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409604.067436224] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409604.072103202] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:13 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:13 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409617.594499160] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:19 WARNING  Transform did not update after motion, checking pose anyway. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:04 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:19 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:19 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:19 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409679.412950789] [moveit_2981870913.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_cylinder_by_id.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py::test_pick_and_place_cylinder_by_id[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. +=================== 1 failed, 1 warning in 203.26s (0:03:23) =================== + + +12:41:31.44 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests +12:41:31.45 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-S1w9dS +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py::test_pick_and_place_sphere_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:40:49 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-38-35-347899-DESKTOP-HG3Q5KV-10396 (testutils.py:33) + +2026-05-10 12:40:49 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [10403] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [10404] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [10405] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-4]: process started with pid [10406] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-5]: process started with pid [10407] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778409515.614720075] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.619154345] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.621406475] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.623664094] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.623692769] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.623698019] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409515.623791181] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.792608955] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.795312913] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.795735947] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.795814867] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.803864122] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805258311] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805294089] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805320689] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805327011] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805398477] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805447941] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409515.868196952] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.120348299] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.120408774] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.122911189] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.135235717] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.136207639] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.136311366] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.143660720] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.144362038] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.147254494] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.148040543] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.148062915] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.152671783] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.163349327] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.163609335] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.164759470] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409516.175369977] [controller_manager]: Overrun might occur, Total time : 9190.454 us (Expected < 2000.000 us) --> Read time : 15.660 us, Update time : 9171.968 us (Switch time : 9091.265 us (Switch chained mode time : 0.350 us, perform mode change time : 13.836 us, Activation time : 9061.198 us, Deactivation time : 0.080 us)), Write time : 2.826 us (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409516.175421586] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.349235 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.175373894] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.177045361] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.177861617] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.177926530] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.178378719] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.188425494] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.191183986] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.191423050] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.191497802] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.192825444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.194319993] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.197267671] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.198233089] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.198280189] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.199245633] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.213014987] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.213264095] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.215392154] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.216944157] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.218302177] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.221433303] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.222533714] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.222575022] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.222846648] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.235104569] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.235343714] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.238847837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.240282088] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.243043875] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.243896505] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.243931903] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.244256955] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.254917437] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.267166806] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267418425] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267527542] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267542100] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267558711] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.269226692] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.276952877] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.278382483] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.281276106] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.380154622] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 10406] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.632501917] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.632553004] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.632959051] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.657232003] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658395588] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658516508] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658549000] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658561914] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.659703920] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.665599106] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.665630485] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.666810828] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.681281194] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.681556833] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.682474947] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.685488327] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.685524946] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.686240521] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.699081171] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.699356043] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.699762706] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.701595372] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.701642372] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.702390398] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.717232571] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.717525236] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.718087374] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.721973524] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.722014822] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.722392034] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.735316282] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.735586341] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.743719188] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.743775104] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.744097888] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.759055439] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.759350254] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.765716551] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.765753751] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.766151948] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.779219845] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.779497812] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.783772545] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.783803133] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.784051925] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.799237540] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.799530431] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 10407] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778409518.151084992] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 10404] (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409518.439158296] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409518.439227378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.386323794] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.251694 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409521.520399618] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.327528 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.334067073] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.994833 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409524.773894503] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.773953906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409525.774256761] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409525.774314040] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409526.774306628] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409526.774358707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409527.774410363] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409527.774469125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409528.774233216] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409528.774290084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409529.121959026] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409529.672912769] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409529.774024087] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409529.774074012] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409530.345270410] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.345321968] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.605410877] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.605487803] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.773949089] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409530.774000967] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409531.774079476] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409531.774132056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.096202708] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.119024499] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409532.670080425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.774031337] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409532.774097132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409533.248410810] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409533.248467438] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409533.774180409] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409533.774243529] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409534.773855321] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409534.773902932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409535.774129847] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409535.774183599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409536.773930962] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409536.773985746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409537.774002583] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409537.774049703] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.774100402] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409538.774162981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409539.774002299] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409539.774056332] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409540.774175370] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409540.774226097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409541.773882336] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409541.773937781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409542.773921897] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409542.773970299] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409543.616000963] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409543.774225965] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409543.774283955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.166978678] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.730365547] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409544.730413689] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409544.774067229] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.774118987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.013145230] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.013212377] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.200213678] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.773905608] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409545.773960132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409546.774092834] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409546.774176894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409547.450396341] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409547.773981303] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409547.774040285] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.001355416] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.689603653] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409548.689655822] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409548.774057429] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.774118725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409549.773998729] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409549.774053152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409550.773967616] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409550.774009235] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409551.773924903] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409551.773978194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409552.773829558] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409552.773874965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409553.774031816] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409553.774090798] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409554.774103308] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409554.774169965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409555.773886465] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409555.773966747] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409556.773943660] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409556.773998254] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409557.773929952] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409557.773976100] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409558.773910633] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409558.773958805] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409558.803377498] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409559.354367550] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409559.773843524] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409559.773897948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409560.204243594] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409560.204303398] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409560.773902643] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409560.773958980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.267119864] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.267192923] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.530217000] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.773939050] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409561.774007270] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409562.773996808] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409562.774036885] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409563.774030510] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409563.774070516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409564.773924516] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409564.773970122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409565.774043119] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409565.774094577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409565.881504984] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409566.432644108] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409566.774034388] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409566.774082289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409567.001155661] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409567.001209824] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409567.773970499] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409567.774009714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409568.774047803] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409568.774107957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409569.773934862] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409569.773999314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409570.774038545] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409570.774128606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409571.773948667] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409571.774003121] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.773967184] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409572.774026367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409573.773927403] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409573.773975955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409574.774001829] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409574.774084987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409575.774054030] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409575.774111670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409576.773970368] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409576.774026785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409577.224523391] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409577.773795712] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409577.773844886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409577.775485538] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409578.381403603] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.381451193] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.523008149] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.523073833] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.770213408] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.773910918] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409578.773950603] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.869387265] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409579.420372240] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409579.773873084] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409579.773921035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409580.088215395] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409580.088265220] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409580.773981697] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409580.774029077] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409581.773926864] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409581.773994713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409582.774008467] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409582.774064303] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409583.773912381] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409583.773966153] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409584.773923530] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409584.773980728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409585.774172284] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409585.774224774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409586.773913606] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409586.773965645] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409587.774073967] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409587.774150071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409588.773970519] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409588.774011727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409589.773813822] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409589.773866962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409590.370976205] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409590.774007937] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409590.774056920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409590.922024831] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409591.774059297] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409591.774110494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409592.095135894] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.095182152] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.273238810] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.273303774] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.430231219] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.774015474] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409592.774065339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409593.774132524] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409593.774195364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409594.774019546] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409594.774070543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409595.774101603] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409595.774167358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409596.774127860] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409596.774199386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409597.390598637] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409597.774190341] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409597.774244915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409597.941608033] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409598.773942856] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409598.773990196] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409598.995449920] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409598.995503962] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409599.773922988] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409599.773972262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409600.774051254] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409600.774108633] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409601.774056185] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409601.774104718] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409602.773976855] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409602.774014907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.774356840] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.774419749] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409604.773930330] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409604.773983180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409605.773950419] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409605.774005474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409606.774020134] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409606.774066161] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409607.774048421] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409607.774106010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409608.774088678] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409608.774166386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409609.055960202] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409609.606941801] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409609.774169493] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409609.774225659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409610.352959853] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.353021380] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.603084781] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.603148121] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.773995161] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409610.774047620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.010219623] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.081872412] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409611.632848546] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.774113350] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409611.774177212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409612.555923196] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409612.556001194] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409612.773967665] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409612.774019694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409613.774180292] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409613.774235928] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409614.773978155] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409614.774031496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409615.774220848] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409615.774264561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409616.774007559] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409616.774065980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409617.773931531] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409617.773982808] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409618.773992303] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409618.774049221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409619.773918614] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409619.773971063] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409620.773903007] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409620.773952912] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409621.773889913] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409621.773940289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409622.622210318] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409622.773946168] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409622.774000221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409623.173189282] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409623.774036844] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409623.774092570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409623.934191606] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409623.934251310] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.237212707] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.237277891] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.396208081] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.745232323] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.774177762] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409624.774231314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409625.296182980] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409625.774109847] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409625.774199647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409626.092293549] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409626.092338926] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409626.773970851] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409626.774023932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409627.774030371] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409627.774082640] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409628.774080559] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409628.774172864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409629.774069059] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409629.774121389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409630.773954062] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409630.773998837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409631.774044663] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409631.774095069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409632.773973584] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409632.774025573] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409633.773996016] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409633.774048075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409634.773894930] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409634.773944574] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409635.774058799] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409635.774162656] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409636.226888766] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409636.773960831] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409636.774000817] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409636.777881425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409637.740621437] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.740680059] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.774168255] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409637.774217709] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.811214763] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.811277813] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409638.186218140] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409638.223759283] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409638.773919151] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409638.773979115] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [1778409518.327323060] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] [INFO] [1778409649.065652015] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:40:49 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:43 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] * Running on http://127.0.0.1:40581 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] * Running on http://172.30.43.138:40581 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.036321843] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.036384503] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.036403499] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.043712542] [moveit_3371273600.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.046311475] [moveit_3371273600.moveit.ros.rdf_loader]: Loaded robot model in 0.00240338 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.046355829] [moveit_3371273600.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.046375286] [moveit_3371273600.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409527.058448036] [moveit_3371273600.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.061223344] [moveit_3371273600.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.123459179] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.123623521] [moveit_3371273600.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.124898750] [moveit_3371273600.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.125970481] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.125987874] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.126110713] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.126607397] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.126692659] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.127500915] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.127516565] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.128106269] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.128671513] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409527.128915657] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409527.128932279] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.300036403] [moveit_3371273600.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.301014070] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302496136] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302515343] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302869185] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302883713] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302913109] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302923148] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302934670] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.303717588] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.306929731] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.306985452] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.309445206] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.309461647] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.310485471] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.344764911] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.348806181] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.348984670] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.349008385] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.349718264] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:47] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Generated 20 grasp options for object Sphere1. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  IK accepted: 20 grasp options. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440496294] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440600302] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440611423] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440646389] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409530.440731441] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.441211763] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.441338995] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.446488090] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.446934037] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.447179864] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602618512] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602656344] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602686080] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602716979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602742788] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604564483] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604608958] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604631030] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604755596] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.605726145] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.605747786] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409532.106457668] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409532.112440632] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:53 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448804011] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448857072] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448864607] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448874095] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409533.448938777] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.449175116] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.449219451] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.452772688] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.454306023] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.454424322] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.454508342] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409543.454607525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005345 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.458383809] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.458448913] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:04 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945762627] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945814846] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945821969] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945833121] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409544.945897012] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.946172616] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.946219815] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.968519447] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.968632532] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.968723655] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:05 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011124691] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011170728] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011203200] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011216535] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011240000] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012589945] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012630311] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012676599] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012810694] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.013452719] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.013466795] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.214092836] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.218451112] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:08 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774712282] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774758810] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774766004] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774775542] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409548.774863740] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.775097735] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.775141398] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.779581262] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.780121454] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.780268975] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.780419981] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409558.780535236] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005350 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.800944996] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.801046529] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:20 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348719480] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348776048] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348786568] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348797649] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409560.348877580] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.349110393] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.349202087] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.354629520] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.354705074] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.354770558] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265629793] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265669729] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265692642] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265701880] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265743740] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266610887] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266651835] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266673797] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266810777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.267480865] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.267498929] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.568315361] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.574499051] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:27 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202704177] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202763680] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202774881] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202786724] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409567.202859843] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.203090471] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.203141337] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.207691473] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.207891599] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.208002108] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.208108185] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409577.208265914] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005080 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.217294971] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.217359463] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505772198] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505845036] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505856238] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505867399] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409578.505934126] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.506195122] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.506238875] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.519388381] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.519471308] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.519554106] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.521956721] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.521985205] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522008178] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522018698] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522037143] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522540840] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522574264] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522595304] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522711555] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.523414977] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.523426589] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.773769996] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.778479835] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:40 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338541153] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338598071] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338606837] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338617979] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409580.338695666] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338927196] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338977692] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.343040253] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.345094366] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.345230180] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.345319614] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409590.345504816] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.006463 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.364763815] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.364833277] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:52 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167800762] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167860736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167869933] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167881415] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409592.167957028] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.168205651] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.168253432] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.181453949] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.181519112] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.181621507] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:52 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270644361] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270680600] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270700197] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270708032] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270733801] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272575279] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272623882] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272650552] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272792833] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.273506849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.273523220] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.474070690] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.478502712] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:56 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:56 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:59 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.040968481] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041020459] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041028465] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041041089] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409599.041110180] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041347050] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041390182] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.045194942] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.045696525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.046111138] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.046183276] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409609.046352226] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004911 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.053176198] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.053236342] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579138144] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579189261] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579196304] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579205512] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409610.579276276] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579495833] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579561318] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.593846621] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.593965648] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.594119229] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602094624] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602122527] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602190786] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602203390] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602221174] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602522210] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602557778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602579329] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602717622] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.603342469] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.603354502] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409611.053987147] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409611.058476286] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:12 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607081664] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607132140] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607139153] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607148491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409612.607228623] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607434765] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607528122] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612424804] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612692873] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612826407] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612947938] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409622.613063829] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005436 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.620162557] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.620222320] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:24 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002605941] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002656427] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002665044] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002675924] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409624.002762188] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002988879] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.003039686] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.021126594] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.021202598] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.021352833] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:24 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236059616] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236113979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236140249] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236149647] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236176688] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236600884] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236653975] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236677009] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236797578] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.237505088] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.237518474] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.438057677] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.442508973] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:26 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204795781] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204854282] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204870944] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204882556] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409626.204960574] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.205173719] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.205214186] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209519433] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209659123] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209800020] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209923350] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409636.210041078] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004782 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.224498991] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.224555618] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769056818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769107575] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769114798] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769124096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409637.769201293] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769403146] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769445507] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.808868039] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809009638] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809145216] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Executing plan (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809784746] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809806868] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809822778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809829661] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809845552] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810603417] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810635037] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810651949] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810762819] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.811477938] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.811489410] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409638.212176999] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409638.216435175] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:39 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.746963428] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747016419] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747023182] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747032860] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409639.747112772] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747319925] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747394677] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409649.194918943] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.750215647] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.750931457] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.751485009] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.751625220] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [WARN] [1778409649.751723812] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004269 seconds (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.769075132] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.769135707] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:13 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:13 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] conn.send({"status": "ok", "result": result}) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self._send_bytes(_ForkingPickler.dumps(obj)) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self._send(header + buf) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] n = write(self._handle, buf) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] BrokenPipeError: [Errno 32] Broken pipe (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:28 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:28 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:28 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:41:31 [ ERROR] [INFO] [1778409688.949668068] [moveit_3371273600.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +_________________ test_pick_and_place_sphere_by_position[ur5e] _________________ + +self = +conn = +method = 'PUT', url = '/graspable/pick_up_object_by_position' +body = b'{"position":{"x":0.0,"y":0.5,"z":0.1},"radius":0.5,"effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' +headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '140'} +retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) +timeout = Timeout(connect=3.05, read=120, total=None), chunked = False +response_conn = +preload_content = False, decode_content = False, enforce_content_length = True + + def _make_request( + self, + conn: BaseHTTPConnection, + method: str, + url: str, + body: _TYPE_BODY | None = None, + headers: typing.Mapping[str, str] | None = None, + retries: Retry | None = None, + timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, + chunked: bool = False, + response_conn: BaseHTTPConnection | None = None, + preload_content: bool = True, + decode_content: bool = True, + enforce_content_length: bool = True, + ) -> BaseHTTPResponse: +  """ +  Perform a request on a given urllib connection object taken from our +  pool. +  +  :param conn: +  a connection from one of our connection pools +  +  :param method: +  HTTP request method (such as GET, POST, PUT, etc.) +  +  :param url: +  The URL to perform the request on. +  +  :param body: +  Data to send in the request body, either :class:`str`, :class:`bytes`, +  an iterable of :class:`str`/:class:`bytes`, or a file-like object. +  +  :param headers: +  Dictionary of custom headers to send, such as User-Agent, +  If-None-Match, etc. If None, pool headers are used. If provided, +  these headers completely replace any pool-specific headers. +  +  :param retries: +  Configure the number of retries to allow before raising a +  :class:`~urllib3.exceptions.MaxRetryError` exception. +  +  Pass ``None`` to retry until you receive a response. Pass a +  :class:`~urllib3.util.retry.Retry` object for fine-grained control +  over different types of retries. +  Pass an integer number to retry connection errors that many times, +  but no other types of errors. Pass zero to never retry. +  +  If ``False``, then retries are disabled and any exception is raised +  immediately. Also, instead of raising a MaxRetryError on redirects, +  the redirect response will be returned. +  +  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. +  +  :param timeout: +  If specified, overrides the default timeout for this one +  request. It may be a float (in seconds) or an instance of +  :class:`urllib3.util.Timeout`. +  +  :param chunked: +  If True, urllib3 will send the body using chunked transfer +  encoding. Otherwise, urllib3 will send the body using the standard +  content-length form. Defaults to False. +  +  :param response_conn: +  Set this to ``None`` if you will handle releasing the connection or +  set the connection to have the response release it. +  +  :param preload_content: +  If True, the response's body will be preloaded during construction. +  +  :param decode_content: +  If True, will attempt to decode the body based on the +  'content-encoding' header. +  +  :param enforce_content_length: +  Enforce content length checking. Body returned by server must match +  value of Content-Length header, if present. Otherwise, raise error. +  """ + self.num_requests += 1 +  + timeout_obj = self._get_timeout(timeout) + timeout_obj.start_connect() + conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) +  + try: + # Trigger any extra validation we need to do. + try: + self._validate_conn(conn) + except (SocketTimeout, BaseSSLError) as e: + self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) + raise +  + # _validate_conn() starts the connection to an HTTPS proxy + # so we need to wrap errors with 'ProxyError' here too. + except ( + OSError, + NewConnectionError, + TimeoutError, + BaseSSLError, + CertificateError, + SSLError, + ) as e: + new_e: Exception = e + if isinstance(e, (BaseSSLError, CertificateError)): + new_e = SSLError(e) + # If the connection didn't successfully connect to it's proxy + # then there + if isinstance( + new_e, (OSError, NewConnectionError, TimeoutError, SSLError) + ) and (conn and conn.proxy and not conn.has_connected_to_proxy): + new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) + raise new_e +  + # conn.request() calls http.client.*.request, not the method in + # urllib3.request. It also calls makefile (recv) on the socket. + try: + conn.request( + method, + url, + body=body, + headers=headers, + chunked=chunked, + preload_content=preload_content, + decode_content=decode_content, + enforce_content_length=enforce_content_length, + ) +  + # We are swallowing BrokenPipeError (errno.EPIPE) since the server is + # legitimately able to close the connection after sending a valid response. + # With this behaviour, the received response is still readable. + except BrokenPipeError: + pass + except OSError as e: + # MacOS/Linux + # EPROTOTYPE and ECONNRESET are needed on macOS + # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ + # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. + if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: + raise +  + # Reset the timeout for the recv() on the socket + read_timeout = timeout_obj.read_timeout +  + if not conn.is_closed: + # In Python 3 socket.py will catch EAGAIN and return None when you + # try and read into the file pointer created by http.client, which + # instead raises a BadStatusLine exception. Instead of catching + # the exception and assuming all BadStatusLine exceptions are read + # timeouts, check for a zero timeout before making the request. + if read_timeout == 0: + raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={read_timeout})" + ) + conn.timeout = read_timeout +  + # Receive the response from the server + try: +> response = conn.getresponse() + ^^^^^^^^^^^^^^^^^^ + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse + httplib_response = super().getresponse() + ^^^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:1428: in getresponse + response.begin() +/usr/lib/python3.12/http/client.py:331: in begin + version, status, reason = self._read_status() + ^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:292: in _read_status + line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +b = + + def readinto(self, b): +  """Read up to len(b) bytes into the writable buffer *b* and return +  the number of bytes read. If the socket is non-blocking and no bytes +  are available, None is returned. +  +  If *b* is non-empty, a 0 return value indicates that the connection +  was shutdown at the other end. +  """ + self._checkClosed() + self._checkReadable() + if self._timeout_occurred: + raise OSError("cannot read from timed out object") + while True: + try: +> return self._sock.recv_into(b) + ^^^^^^^^^^^^^^^^^^^^^^^ +E TimeoutError: timed out + +/usr/lib/python3.12/socket.py:707: TimeoutError + +The above exception was the direct cause of the following exception: + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: +> resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen + retries = retries.increment( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment + raise reraise(type(error), error, _stacktrace) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise + raise value +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen + response = self._make_request( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request + self._raise_timeout(err=e, url=url, timeout_value=read_timeout) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_position' +timeout_value = 120 + + def _raise_timeout( + self, + err: BaseSSLError | OSError | SocketTimeout, + url: str, + timeout_value: _TYPE_TIMEOUT | None, + ) -> None: +  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" +  + if isinstance(err, SocketTimeout): +> raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={timeout_value})" + ) from err +E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=40581): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError + +During handling of the above exception, another exception occurred: + +method = )> +url = 'http://0.0.0.0:40581/graspable/pick_up_object_by_position' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: +> resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + +src/python/arcor2_web/rest.py:259: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put + return request("put", url, data=data, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request + return session.request(method=method, url=url, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request + resp = self.send(prep, **send_kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send + r = adapter.send(request, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: + resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) +  + except (ProtocolError, OSError) as err: + raise ConnectionError(err, request=request) +  + except MaxRetryError as e: + if isinstance(e.reason, ConnectTimeoutError): + # TODO: Remove this in 3.0.0: see #2811 + if not isinstance(e.reason, NewConnectionError): + raise ConnectTimeout(e, request=request) +  + if isinstance(e.reason, ResponseError): + raise RetryError(e, request=request) +  + if isinstance(e.reason, _ProxyError): + raise ProxyError(e, request=request) +  + if isinstance(e.reason, _SSLError): + # This branch is for urllib3 v1.22 and later. + raise SSLError(e, request=request) +  + raise ConnectionError(e, request=request) +  + except ClosedPoolError as e: + raise ConnectionError(e, request=request) +  + except _ProxyError as e: + raise ProxyError(e) +  + except (_SSLError, _HTTPError) as e: + if isinstance(e, _SSLError): + # This branch is for urllib3 versions earlier than v1.22 + raise SSLError(e, request=request) + elif isinstance(e, ReadTimeoutError): +> raise ReadTimeout(e, request=request) +E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=40581): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout + +The above exception was the direct cause of the following exception: + +start_processes = Urls(ros_domain_id='156', robot_url='http://0.0.0.0:40581', storage_url='http://127.0.0.1:36483') + + @pytest.mark.timeout(321) + def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + object = Sphere("Sphere1", 0.1) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position + rest.call( +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +method = )> +url = 'http://0.0.0.0:40581/graspable/pick_up_object_by_position' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: + resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + except requests.exceptions.RequestException as e: + logger.debug("Request failed.", exc_info=True) + # TODO would be good to provide more meaningful message but the original one could be very very long +> raise RestException("Catastrophic system error.") from e +E arcor2_web.rest.RestException: Catastrophic system error. + +src/python/arcor2_web/rest.py:269: RestException +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-38-35-347899-DESKTOP-HG3Q5KV-10396 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [10403] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [10404] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [10405] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [10406] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [10407] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409515.614720075] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.619154345] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.621406475] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.623664094] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.623692769] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.623698019] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409515.623791181] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.792608955] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.795312913] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.795735947] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.795814867] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.803864122] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805258311] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805294089] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805320689] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805327011] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805398477] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805447941] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409515.868196952] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.120348299] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.120408774] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.122911189] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.135235717] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.136207639] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.136311366] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.143660720] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.144362038] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.147254494] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.148040543] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.148062915] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.152671783] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.163349327] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.163609335] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.164759470] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409516.175369977] [controller_manager]: Overrun might occur, Total time : 9190.454 us (Expected < 2000.000 us) --> Read time : 15.660 us, Update time : 9171.968 us (Switch time : 9091.265 us (Switch chained mode time : 0.350 us, perform mode change time : 13.836 us, Activation time : 9061.198 us, Deactivation time : 0.080 us)), Write time : 2.826 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409516.175421586] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.349235 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.175373894] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.177045361] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.177861617] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.177926530] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.178378719] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.188425494] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.191183986] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.191423050] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.191497802] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.192825444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.194319993] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.197267671] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.198233089] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.198280189] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.199245633] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.213014987] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.213264095] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.215392154] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.216944157] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.218302177] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.221433303] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.222533714] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.222575022] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.222846648] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.235104569] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.235343714] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.238847837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.240282088] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.243043875] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.243896505] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.243931903] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.244256955] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.254917437] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.267166806] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267418425] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267527542] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267542100] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267558711] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.269226692] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.276952877] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.278382483] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.281276106] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.380154622] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 10406] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.632501917] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.632553004] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.632959051] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.657232003] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658395588] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658516508] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658549000] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658561914] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.659703920] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.665599106] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.665630485] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.666810828] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.681281194] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.681556833] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.682474947] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.685488327] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.685524946] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.686240521] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.699081171] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.699356043] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.699762706] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.701595372] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.701642372] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.702390398] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.717232571] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.717525236] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.718087374] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.721973524] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.722014822] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.722392034] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.735316282] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.735586341] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.743719188] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.743775104] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.744097888] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.759055439] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.759350254] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.765716551] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.765753751] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.766151948] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.779219845] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.779497812] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.783772545] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.783803133] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.784051925] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.799237540] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.799530431] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 10407] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409518.151084992] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 10404] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409518.439158296] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409518.439227378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.386323794] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.251694 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409521.520399618] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.327528 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.334067073] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.994833 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409524.773894503] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.773953906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409525.774256761] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409525.774314040] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409526.774306628] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409526.774358707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409527.774410363] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409527.774469125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409528.774233216] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409528.774290084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409529.121959026] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409529.672912769] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409529.774024087] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409529.774074012] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409530.345270410] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.345321968] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.605410877] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.605487803] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.773949089] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409530.774000967] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409531.774079476] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409531.774132056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.096202708] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.119024499] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409532.670080425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.774031337] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409532.774097132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409533.248410810] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409533.248467438] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409533.774180409] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409533.774243529] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409534.773855321] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409534.773902932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409535.774129847] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409535.774183599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409536.773930962] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409536.773985746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409537.774002583] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409537.774049703] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.774100402] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409538.774162981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409539.774002299] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409539.774056332] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409540.774175370] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409540.774226097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409541.773882336] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409541.773937781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409542.773921897] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409542.773970299] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409543.616000963] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409543.774225965] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409543.774283955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.166978678] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.730365547] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409544.730413689] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409544.774067229] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.774118987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.013145230] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.013212377] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.200213678] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.773905608] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409545.773960132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409546.774092834] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409546.774176894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409547.450396341] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409547.773981303] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409547.774040285] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.001355416] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.689603653] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409548.689655822] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409548.774057429] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.774118725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409549.773998729] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409549.774053152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409550.773967616] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409550.774009235] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409551.773924903] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409551.773978194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409552.773829558] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409552.773874965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409553.774031816] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409553.774090798] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409554.774103308] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409554.774169965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409555.773886465] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409555.773966747] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409556.773943660] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409556.773998254] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409557.773929952] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409557.773976100] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409558.773910633] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409558.773958805] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409558.803377498] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409559.354367550] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409559.773843524] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409559.773897948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409560.204243594] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409560.204303398] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409560.773902643] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409560.773958980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.267119864] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.267192923] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.530217000] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.773939050] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409561.774007270] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409562.773996808] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409562.774036885] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409563.774030510] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409563.774070516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409564.773924516] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409564.773970122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409565.774043119] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409565.774094577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409565.881504984] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409566.432644108] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409566.774034388] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409566.774082289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409567.001155661] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409567.001209824] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409567.773970499] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409567.774009714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409568.774047803] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409568.774107957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409569.773934862] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409569.773999314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409570.774038545] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409570.774128606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409571.773948667] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409571.774003121] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.773967184] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409572.774026367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409573.773927403] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409573.773975955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409574.774001829] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409574.774084987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409575.774054030] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409575.774111670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409576.773970368] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409576.774026785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409577.224523391] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409577.773795712] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409577.773844886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409577.775485538] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409578.381403603] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.381451193] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.523008149] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.523073833] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.770213408] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.773910918] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409578.773950603] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.869387265] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409579.420372240] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409579.773873084] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409579.773921035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409580.088215395] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409580.088265220] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409580.773981697] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409580.774029077] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409581.773926864] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409581.773994713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409582.774008467] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409582.774064303] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409583.773912381] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409583.773966153] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409584.773923530] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409584.773980728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409585.774172284] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409585.774224774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409586.773913606] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409586.773965645] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409587.774073967] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409587.774150071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409588.773970519] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409588.774011727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409589.773813822] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409589.773866962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409590.370976205] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409590.774007937] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409590.774056920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409590.922024831] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409591.774059297] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409591.774110494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409592.095135894] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.095182152] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.273238810] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.273303774] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.430231219] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.774015474] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409592.774065339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409593.774132524] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409593.774195364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409594.774019546] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409594.774070543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409595.774101603] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409595.774167358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409596.774127860] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409596.774199386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409597.390598637] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409597.774190341] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409597.774244915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409597.941608033] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409598.773942856] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409598.773990196] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409598.995449920] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409598.995503962] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409599.773922988] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409599.773972262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409600.774051254] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409600.774108633] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409601.774056185] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409601.774104718] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409602.773976855] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409602.774014907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.774356840] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.774419749] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409604.773930330] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409604.773983180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409605.773950419] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409605.774005474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409606.774020134] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409606.774066161] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409607.774048421] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409607.774106010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409608.774088678] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409608.774166386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409609.055960202] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409609.606941801] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409609.774169493] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409609.774225659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409610.352959853] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.353021380] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.603084781] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.603148121] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.773995161] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409610.774047620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.010219623] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.081872412] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409611.632848546] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.774113350] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409611.774177212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409612.555923196] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409612.556001194] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409612.773967665] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409612.774019694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409613.774180292] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409613.774235928] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409614.773978155] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409614.774031496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409615.774220848] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409615.774264561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409616.774007559] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409616.774065980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409617.773931531] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409617.773982808] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409618.773992303] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409618.774049221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409619.773918614] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409619.773971063] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409620.773903007] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409620.773952912] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409621.773889913] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409621.773940289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409622.622210318] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409622.773946168] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409622.774000221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409623.173189282] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409623.774036844] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409623.774092570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409623.934191606] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409623.934251310] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.237212707] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.237277891] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.396208081] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.745232323] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.774177762] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409624.774231314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409625.296182980] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409625.774109847] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409625.774199647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409626.092293549] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409626.092338926] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409626.773970851] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409626.774023932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409627.774030371] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409627.774082640] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409628.774080559] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409628.774172864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409629.774069059] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409629.774121389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409630.773954062] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409630.773998837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409631.774044663] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409631.774095069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409632.773973584] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409632.774025573] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409633.773996016] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409633.774048075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409634.773894930] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409634.773944574] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409635.774058799] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409635.774162656] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409636.226888766] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409636.773960831] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409636.774000817] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409636.777881425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409637.740621437] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.740680059] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.774168255] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409637.774217709] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.811214763] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.811277813] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409638.186218140] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409638.223759283] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409638.773919151] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409638.773979115] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409518.327323060] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409649.065652015] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:43 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:40581 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:40581 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.036321843] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.036384503] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.036403499] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.043712542] [moveit_3371273600.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.046311475] [moveit_3371273600.moveit.ros.rdf_loader]: Loaded robot model in 0.00240338 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.046355829] [moveit_3371273600.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.046375286] [moveit_3371273600.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409527.058448036] [moveit_3371273600.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.061223344] [moveit_3371273600.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.123459179] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.123623521] [moveit_3371273600.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.124898750] [moveit_3371273600.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.125970481] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.125987874] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.126110713] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.126607397] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.126692659] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.127500915] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.127516565] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.128106269] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.128671513] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409527.128915657] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409527.128932279] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.300036403] [moveit_3371273600.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.301014070] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302496136] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302515343] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302869185] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302883713] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302913109] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302923148] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302934670] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.303717588] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.306929731] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.306985452] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.309445206] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.309461647] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.310485471] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.344764911] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.348806181] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.348984670] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.349008385] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.349718264] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:47] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Generated 20 grasp options for object Sphere1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  IK accepted: 20 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440496294] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440600302] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440611423] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440646389] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409530.440731441] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.441211763] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.441338995] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.446488090] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.446934037] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.447179864] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602618512] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602656344] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602686080] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602716979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602742788] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604564483] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604608958] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604631030] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604755596] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.605726145] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.605747786] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409532.106457668] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409532.112440632] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:53 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448804011] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448857072] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448864607] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448874095] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409533.448938777] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.449175116] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.449219451] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.452772688] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.454306023] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.454424322] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.454508342] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409543.454607525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005345 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.458383809] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.458448913] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945762627] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945814846] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945821969] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945833121] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409544.945897012] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.946172616] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.946219815] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.968519447] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.968632532] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.968723655] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011124691] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011170728] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011203200] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011216535] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011240000] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012589945] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012630311] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012676599] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012810694] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.013452719] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.013466795] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.214092836] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.218451112] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:08 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774712282] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774758810] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774766004] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774775542] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409548.774863740] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.775097735] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.775141398] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.779581262] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.780121454] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.780268975] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.780419981] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409558.780535236] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005350 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.800944996] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.801046529] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348719480] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348776048] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348786568] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348797649] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409560.348877580] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.349110393] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.349202087] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.354629520] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.354705074] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.354770558] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265629793] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265669729] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265692642] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265701880] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265743740] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266610887] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266651835] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266673797] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266810777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.267480865] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.267498929] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.568315361] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.574499051] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202704177] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202763680] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202774881] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202786724] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409567.202859843] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.203090471] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.203141337] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.207691473] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.207891599] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.208002108] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.208108185] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409577.208265914] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005080 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.217294971] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.217359463] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505772198] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505845036] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505856238] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505867399] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409578.505934126] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.506195122] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.506238875] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.519388381] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.519471308] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.519554106] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.521956721] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.521985205] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522008178] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522018698] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522037143] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522540840] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522574264] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522595304] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522711555] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.523414977] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.523426589] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.773769996] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.778479835] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:40 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338541153] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338598071] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338606837] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338617979] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409580.338695666] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338927196] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338977692] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.343040253] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.345094366] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.345230180] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.345319614] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409590.345504816] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.006463 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.364763815] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.364833277] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167800762] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167860736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167869933] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167881415] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409592.167957028] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.168205651] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.168253432] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.181453949] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.181519112] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.181621507] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270644361] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270680600] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270700197] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270708032] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270733801] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272575279] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272623882] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272650552] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272792833] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.273506849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.273523220] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.474070690] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.478502712] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:56 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:56 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:59 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.040968481] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041020459] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041028465] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041041089] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409599.041110180] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041347050] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041390182] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.045194942] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.045696525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.046111138] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.046183276] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409609.046352226] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004911 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.053176198] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.053236342] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579138144] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579189261] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579196304] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579205512] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409610.579276276] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579495833] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579561318] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.593846621] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.593965648] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.594119229] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602094624] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602122527] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602190786] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602203390] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602221174] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602522210] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602557778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602579329] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602717622] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.603342469] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.603354502] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409611.053987147] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409611.058476286] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607081664] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607132140] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607139153] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607148491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409612.607228623] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607434765] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607528122] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612424804] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612692873] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612826407] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612947938] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409622.613063829] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005436 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.620162557] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.620222320] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:24 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002605941] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002656427] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002665044] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002675924] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409624.002762188] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002988879] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.003039686] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.021126594] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.021202598] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.021352833] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:24 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236059616] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236113979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236140249] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236149647] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236176688] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236600884] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236653975] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236677009] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236797578] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.237505088] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.237518474] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.438057677] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.442508973] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:26 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204795781] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204854282] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204870944] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204882556] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409626.204960574] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.205173719] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.205214186] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209519433] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209659123] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209800020] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209923350] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409636.210041078] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004782 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.224498991] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.224555618] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769056818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769107575] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769114798] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769124096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409637.769201293] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769403146] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769445507] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.808868039] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809009638] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809145216] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809784746] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809806868] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809822778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809829661] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809845552] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810603417] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810635037] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810651949] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810762819] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.811477938] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.811489410] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409638.212176999] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409638.216435175] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:39 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.746963428] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747016419] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747023182] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747032860] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409639.747112772] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747319925] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747394677] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409649.194918943] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.750215647] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.750931457] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.751485009] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.751625220] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409649.751723812] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004269 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.769075132] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.769135707] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:13 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:13 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 conn.send({"status": "ok", "result": result}) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send_bytes(_ForkingPickler.dumps(obj)) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send(header + buf) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 n = write(self._handle, buf) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 BrokenPipeError: [Errno 32] Broken pipe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:28 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:28 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:28 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409688.949668068] [moveit_3371273600.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_sphere_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py::test_pick_and_place_sphere_by_position[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. +=================== 1 failed, 1 warning in 176.33s (0:02:56) =================== + + +12:41:35.97 [INFO] Long running tasks: + 537.77s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:42:06.01 [INFO] Long running tasks: + 567.81s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:42:36.04 [INFO] Long running tasks: + 597.84s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:42:38.30 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:44:06.17 [INFO] Long running tasks: + 87.87s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:44:36.21 [INFO] Long running tasks: + 117.91s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:45:06.25 [INFO] Long running tasks: + 147.94s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:45:36.28 [INFO] Long running tasks: + 177.98s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:46:01.14 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests +12:46:01.15 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - failed (exit code 1). +============================= test session starts ============================== +platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 +rootdir: /tmp/pants-sandbox-WzZgMH +configfile: pytest.ini +asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function +collected 1 item + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py::test_pick_and_place_cylinder_by_position[ur5e] FAILED [100%] +------------------------------ live log teardown ------------------------------- +2026-05-10 12:45:11 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-42-39-321904-DESKTOP-HG3Q5KV-11687 (testutils.py:33) + +2026-05-10 12:45:11 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [11697] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [11698] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [11699] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-4]: process started with pid [11700] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-5]: process started with pid [11701] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [robot_state_publisher-2] [INFO] [1778409759.572370442] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.575114270] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.577347037] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.579718092] [controller_manager]: update rate is 500 Hz (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.579745054] [controller_manager]: Overruns handling is : enabled (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.579750444] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409759.579841808] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.587985256] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.590568079] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.590936359] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.591006081] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.597676025] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598778450] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598806593] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598826561] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598830839] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598875915] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598906874] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409759.796350801] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.048086102] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.048137820] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.050369014] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.063311131] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.064269262] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.064390081] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.071389411] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.072355622] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.075025881] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.075615661] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.075650738] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.079958879] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.090902207] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.091127927] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.092898246] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409760.102387266] [controller_manager]: Overrun might occur, Total time : 8218.786 us (Expected < 2000.000 us) --> Read time : 12.393 us, Update time : 8204.279 us (Switch time : 8164.122 us (Switch chained mode time : 0.221 us, perform mode change time : 9.688 us, Activation time : 8137.792 us, Deactivation time : 0.080 us)), Write time : 2.114 us (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409760.102436369] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.361093 ms (missed cycles : 5). (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.102400562] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.104897746] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.105534206] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.105589781] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.105927302] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.113736721] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.114973832] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.115183214] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.115243960] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.116900007] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.118259791] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.121255458] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.121986862] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.122016658] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.122926768] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.136838878] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.137019340] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.138912768] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.140936653] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.142264922] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.144992661] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.145726586] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.145756282] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.146024652] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.157190588] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.157438333] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.160904966] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.162301847] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.165009810] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.165995709] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.166026167] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.166321508] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.175373999] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.187133253] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187351883] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187471952] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187490887] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187507800] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.189338703] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.196883635] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.198482022] [controller_manager]: Successfully switched controllers! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.201254175] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.300896948] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 11700] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.552925686] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.552971624] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.553297012] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.577285509] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578372249] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578501324] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578534357] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578549736] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.579896739] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.585454410] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.585485018] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.586866323] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.599101881] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.599337505] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.599933978] [forward_velocity_controller]: configure successful (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.601466960] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.601501486] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.602178979] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.615091618] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.615361431] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.615856091] [forward_position_controller]: configure successful (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.617339460] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.617374387] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.617963511] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.631015802] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.631256749] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.631834047] [forward_effort_controller]: configure successful (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.633593280] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.633629018] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.633921754] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.647113599] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.647375633] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.657399630] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.657454774] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.657766216] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.671224127] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.671490659] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.677654796] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.677686366] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.677956966] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.691288188] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.691542766] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.693839549] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.693868144] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.694081689] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.707342912] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.707635995] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 11701] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [robot_state_publisher-2] [INFO] [1778409762.154008266] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 11698] (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409762.441361682] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409762.441407409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409764.366208387] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.132930 ms (missed cycles : 4). (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409765.470213376] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.138190 ms (missed cycles : 3). (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409767.370228047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.152900 ms (missed cycles : 2). (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409767.764106413] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409767.764159974] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409768.764221564] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409768.764270436] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409769.764353752] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409769.764403917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409770.764279850] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409770.764331438] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409771.764296829] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409771.764348337] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409772.764151065] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409772.764195329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409773.764308030] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409773.764363706] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409774.764317523] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409774.764376144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409775.764264462] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409775.764315809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409776.764271604] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409776.764333021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409777.764201295] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409777.764256730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409778.764213293] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409778.764258990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409779.764320319] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409779.764402566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409780.764376270] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409780.764428580] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409781.764234109] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409781.764284775] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409782.764221240] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409782.764277897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409783.764200595] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409783.764253175] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409784.764206302] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409784.764260665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409785.764284875] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409785.764335051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409786.764236769] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409786.764274701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409787.764304257] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409787.764419431] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409788.764223979] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409788.764272491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409789.764202853] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409789.764256505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409790.764193047] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409790.764256858] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409791.764231436] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409791.764286090] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409792.560261504] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409792.764167109] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409792.764212796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409793.111256952] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409793.764242579] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409793.764315818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409794.664282440] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409794.664339488] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409794.764221255] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409794.764293623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409795.764231286] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409795.764283425] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409796.219305492] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409796.219378792] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409796.764140746] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409796.764188406] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409797.686221356] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409797.764151306] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409797.764194769] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409798.764168300] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409798.764231409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409799.764138819] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409799.764192160] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409800.764216317] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409800.764271802] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409801.764173245] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409801.764225634] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409802.735369275] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409802.764235980] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409802.764284542] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409803.286375253] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409803.764228797] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409803.764289833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409804.202246564] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409804.202289115] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409804.764350223] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409804.764403744] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409805.764143247] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409805.764191188] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409806.764178860] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409806.764253532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409807.764154195] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409807.764204591] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409808.764180726] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409808.764230901] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409809.764191153] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409809.764250566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409810.764180819] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409810.764249470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409811.764191596] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409811.764238565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409812.764181212] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409812.764227530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409813.764191675] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409813.764249625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409814.764181026] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409814.764229659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409815.764183111] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409815.764235070] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409816.764308311] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409816.764362654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409816.782527509] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409817.333485734] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409817.764216117] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409817.764269418] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409818.764184825] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409818.764234409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409819.267339971] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409819.267390677] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409819.764203527] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409819.764260024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.606993302] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.607043196] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.748194354] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.764149736] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409820.764189692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409821.764104533] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409821.764161561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409822.764151759] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409822.764223735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409823.764083220] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409823.764136411] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409824.764143506] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409824.764198079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409824.766423494] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409825.317389150] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409825.764192539] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409825.764233758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409826.764187190] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409826.764257223] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409827.764097767] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409827.764145818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409828.024658497] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409828.024714844] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409828.764153389] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409828.764204746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409829.764194344] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409829.764249919] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409830.764183563] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409830.764231384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409831.764209764] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409831.764259027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409832.764289690] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409832.764339715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409833.764213431] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409833.764263245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409834.764203131] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409834.764252324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409835.764172697] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409835.764224516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409836.764150921] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409836.764198401] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409837.764182699] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409837.764239306] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409838.340023718] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409838.764146377] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409838.764190791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409838.891018373] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409839.764202651] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409839.764240242] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409840.083369699] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.083423341] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.764216716] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409840.764281318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.871145575] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.871208274] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409841.764198548] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409841.764251038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409842.668214952] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409842.764281857] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409842.764335960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409843.764142655] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409843.764192810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409844.764157054] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409844.764230965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409845.764392813] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409845.764455562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409846.764194145] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409846.764248037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409847.764208528] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409847.764249636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409848.764175566] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409848.764228657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409849.533039064] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409849.764145599] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409849.764197628] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409850.083961123] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409850.764292220] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409850.764342245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409850.864032152] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409850.864078340] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409851.764195297] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409851.764274027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409852.764175676] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409852.764232103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409853.764172091] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409853.764220353] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409854.764177934] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409854.764228691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409855.764216244] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409855.764271629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409856.764091563] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409856.764138863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409857.764163193] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409857.764211815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409858.764203304] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409858.764254541] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409859.764161922] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409859.764208641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409860.764203237] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409860.764255276] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409861.069795227] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409861.620866347] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409861.764215221] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409861.764266990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409862.438528062] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.438587956] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.764335855] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409862.764378456] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.767017921] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.767074939] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.962206344] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409863.764169838] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409863.764219502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409864.764191833] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409864.764242560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409865.764171424] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409865.764229274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409866.764185664] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409866.764240809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409867.764406392] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409867.764454714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409867.960177944] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409868.511163799] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409868.764178218] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409868.764236378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409869.255438544] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409869.255499320] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409869.764144008] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409869.764197760] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409870.764189251] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409870.764241730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409871.764179272] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409871.764257400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409872.764179042] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409872.764254194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409873.764170001] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409873.764238240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409874.764203882] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409874.764273675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409875.764251384] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409875.764307981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409876.764219542] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409876.764274557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409877.764198201] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409877.764267423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409878.764162437] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409878.764212001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409879.764204727] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409879.764260693] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409879.948926906] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409880.499862929] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409880.764196880] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409880.764254880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409881.764169136] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409881.764239410] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409881.983318541] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409882.534276948] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409882.764203731] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409882.764260399] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409883.387738055] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.387793610] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.551063022] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.551120781] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.760222550] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.764180751] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409883.764220707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409884.764142597] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409884.764195047] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409885.764155987] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409885.764202947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409886.764160882] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409886.764204946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409887.764237674] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409887.764295594] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409888.764190966] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409888.764249117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409889.363671580] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409889.764147673] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409889.764195784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409889.914686842] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409890.764151902] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409890.764221915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409890.828126416] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409890.828182784] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409891.764203500] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409891.764251812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409892.764138920] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409892.764187272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409893.764178169] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409893.764250006] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409894.764175368] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409894.764223350] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409895.764221848] [controller_manager]: Received robot description from topic. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409895.764274678] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [1778409762.319032737] [robot_state_publisher]: Robot initialized (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] [INFO] [1778409911.373843058] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) +2026-05-10 12:45:11 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:47 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] * Debug mode: off (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] * Running on http://127.0.0.1:58727 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] * Running on http://172.30.43.138:58727 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] Press CTRL+C to quit (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.257718930] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.257775468] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.257788693] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.264980653] [moveit_1502777475.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.266709778] [moveit_1502777475.moveit.ros.rdf_loader]: Loaded robot model in 0.00161847 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.266735237] [moveit_1502777475.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.266743102] [moveit_1502777475.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409772.279139666] [moveit_1502777475.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.281573511] [moveit_1502777475.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.334319664] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.334435014] [moveit_1502777475.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.335323071] [moveit_1502777475.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.335873668] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.335886742] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.336014686] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.336392794] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.336480110] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337085761] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337097563] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337542359] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337993165] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409772.338242710] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409772.338258289] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.820442676] [moveit_1502777475.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.821083354] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822069167] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822079546] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822369427] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822385167] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822410565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822416186] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822426466] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.823013311] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.825261203] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.825276572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.827036275] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.827049400] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.827853978] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.856359461] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.859791132] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.859954893] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.859977827] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.860833573] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:55 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:55 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:43:09] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:43:09] "GET /joints HTTP/1.1" 200 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:43:10] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:10 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:10 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:11 INFO  Generated 20 grasp options for object Cyl1. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  IK accepted: 20 grasp options. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:14 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:14 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:15 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:15 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153443391] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153563640] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153577316] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153595170] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409796.153662067] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.154085602] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.154206211] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.169621863] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.170072870] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.170416763] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Executing plan (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216943760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216965231] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216983004] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216989677] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.217007110] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218558112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218597668] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218622855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218775075] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.219582153] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.219600488] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409797.720152829] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409797.724501172] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:18 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:18 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:19 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:19 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:20 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:20 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:25 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430164123] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430238374] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430252491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430265947] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409805.430344977] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430586536] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430635269] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.433369314] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.433788250] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.433897812] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.434094120] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409815.434251880] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003569 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.442111916] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.442221494] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:35 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:35 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:36 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:36 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:38 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:38 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584147221] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584203178] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584211964] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584223005] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409820.584299541] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584518948] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584598829] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.597357623] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.597428688] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.597522907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Executing plan (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604442585] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604467592] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604485016] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604493532] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604512668] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606562112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606591648] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606612728] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606725563] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.607298807] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.607312493] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.757777293] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.762481005] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:41 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:41 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:48 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309517858] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309572281] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309580797] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309592019] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409828.309701827] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309951271] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.310044769] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.313605646] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.313763095] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.314096699] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.314217483] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409838.314339981] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004222 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.335261686] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.335320918] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:59 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:59 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.334937564] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.334986878] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.334994031] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.335004962] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409840.335072430] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.335303730] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.335344007] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.361554961] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.362156544] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.362570550] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Executing plan (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868328733] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868368077] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868408855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868422000] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868454421] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870634349] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870682330] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870704953] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870838467] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.871434003] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.871448691] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409842.671872612] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409842.676467122] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:07 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:07 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:08 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:08 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:09 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:09 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:11 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:11 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:11 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005531569] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005608716] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005620558] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005633563] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409851.005712773] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005958300] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.006008856] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008170904] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008356352] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008610689] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008726895] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409861.008897064] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.002831 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.027596805] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.027655586] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724713760] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724773343] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724782410] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724793952] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409862.724870548] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.725097599] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.725148887] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.741512678] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.741581709] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.741639619] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Executing plan (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765078426] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765101149] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765115166] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765122760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765139312] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766605713] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766639918] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766661479] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766773902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.767293940] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.767308498] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.967715412] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.972475057] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:25 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:25 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:26 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:26 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:27 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:27 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:28 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:28 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:29 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:29 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:29 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903117283] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903168130] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903175123] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903184341] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409869.903280754] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903521070] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903615410] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.906350239] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.906886684] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.907037235] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.907146348] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409879.907300711] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003617 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.930359365] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.930435400] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:39 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:39 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:43 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544616182] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544667620] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544674794] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544684552] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409883.544758092] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544983991] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.545065927] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.548860186] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.548946149] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549026222] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:43 INFO  Executing plan (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549221883] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549240869] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549254145] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549261188] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549276377] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550556860] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550584162] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550612315] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550723767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.551306820] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.551322309] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.801675456] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.806462571] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:44 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:44 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:45 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:45 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:48 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:48 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:49 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:49 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:50 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:50 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:51 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:51 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:51 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023659043] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023709839] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023716782] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023726441] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409891.023790152] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.024005240] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.024046759] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.026011506] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.027183142] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.027674426] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.027763870] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409901.027877612] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003787 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.030747966] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.030807449] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478695882] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478746709] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478753822] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478763250] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409902.478825398] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.479049083] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.479089971] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.491502647] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.491578932] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.491652091] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Executing plan (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658462590] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658489952] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658507485] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658516202] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658536400] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660578857] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660618292] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660643460] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660760553] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.661270336] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.661281607] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.861545943] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.866470690] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:03 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:03 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:04 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:04 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:05 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:05 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:06 INFO  Robot mode: 7 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:06 INFO  Program running: True (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:07 INFO  Planning trajectory (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578555476] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578604820] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578613286] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578622273] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409907.578684071] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578887607] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578929567] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409911.502946530] [rclcpp]: signal_handler(signum=15) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.582781701] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.582860536] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.583501359] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.583585333] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [WARN] [1778409917.583696751] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004731 seconds (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.604220980] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.604280282] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:43 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] result = runtime.attach_object( (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:43 ERROR  ROS worker command failed. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] conn.send({"status": "ok", "result": result}) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self._send_bytes(_ForkingPickler.dumps(obj)) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self._send(header + buf) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] n = write(self._handle, buf) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] BrokenPipeError: [Errno 32] Broken pipe (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:58 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] active = self._active_controllers() (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] This error state is being overwritten: (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] with this new error message: (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] <<< (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:58 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:58 ERROR  Failed to shutdown rclpy. (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] rclpy.shutdown() (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] _shutdown(context=context) (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] context.shutdown() (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] self.__context.shutdown() (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) +2026-05-10 12:46:00 [ ERROR] [INFO] [1778409958.792785146] [moveit_1502777475.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) + + +=================================== FAILURES =================================== +________________ test_pick_and_place_cylinder_by_position[ur5e] ________________ + +self = +conn = +method = 'PUT', url = '/graspable/pick_up_object_by_position' +body = b'{"position":{"x":0.0,"y":0.5,"z":0.1},"radius":0.5,"effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' +headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '140'} +retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) +timeout = Timeout(connect=3.05, read=120, total=None), chunked = False +response_conn = +preload_content = False, decode_content = False, enforce_content_length = True + + def _make_request( + self, + conn: BaseHTTPConnection, + method: str, + url: str, + body: _TYPE_BODY | None = None, + headers: typing.Mapping[str, str] | None = None, + retries: Retry | None = None, + timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, + chunked: bool = False, + response_conn: BaseHTTPConnection | None = None, + preload_content: bool = True, + decode_content: bool = True, + enforce_content_length: bool = True, + ) -> BaseHTTPResponse: +  """ +  Perform a request on a given urllib connection object taken from our +  pool. +  +  :param conn: +  a connection from one of our connection pools +  +  :param method: +  HTTP request method (such as GET, POST, PUT, etc.) +  +  :param url: +  The URL to perform the request on. +  +  :param body: +  Data to send in the request body, either :class:`str`, :class:`bytes`, +  an iterable of :class:`str`/:class:`bytes`, or a file-like object. +  +  :param headers: +  Dictionary of custom headers to send, such as User-Agent, +  If-None-Match, etc. If None, pool headers are used. If provided, +  these headers completely replace any pool-specific headers. +  +  :param retries: +  Configure the number of retries to allow before raising a +  :class:`~urllib3.exceptions.MaxRetryError` exception. +  +  Pass ``None`` to retry until you receive a response. Pass a +  :class:`~urllib3.util.retry.Retry` object for fine-grained control +  over different types of retries. +  Pass an integer number to retry connection errors that many times, +  but no other types of errors. Pass zero to never retry. +  +  If ``False``, then retries are disabled and any exception is raised +  immediately. Also, instead of raising a MaxRetryError on redirects, +  the redirect response will be returned. +  +  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. +  +  :param timeout: +  If specified, overrides the default timeout for this one +  request. It may be a float (in seconds) or an instance of +  :class:`urllib3.util.Timeout`. +  +  :param chunked: +  If True, urllib3 will send the body using chunked transfer +  encoding. Otherwise, urllib3 will send the body using the standard +  content-length form. Defaults to False. +  +  :param response_conn: +  Set this to ``None`` if you will handle releasing the connection or +  set the connection to have the response release it. +  +  :param preload_content: +  If True, the response's body will be preloaded during construction. +  +  :param decode_content: +  If True, will attempt to decode the body based on the +  'content-encoding' header. +  +  :param enforce_content_length: +  Enforce content length checking. Body returned by server must match +  value of Content-Length header, if present. Otherwise, raise error. +  """ + self.num_requests += 1 +  + timeout_obj = self._get_timeout(timeout) + timeout_obj.start_connect() + conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) +  + try: + # Trigger any extra validation we need to do. + try: + self._validate_conn(conn) + except (SocketTimeout, BaseSSLError) as e: + self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) + raise +  + # _validate_conn() starts the connection to an HTTPS proxy + # so we need to wrap errors with 'ProxyError' here too. + except ( + OSError, + NewConnectionError, + TimeoutError, + BaseSSLError, + CertificateError, + SSLError, + ) as e: + new_e: Exception = e + if isinstance(e, (BaseSSLError, CertificateError)): + new_e = SSLError(e) + # If the connection didn't successfully connect to it's proxy + # then there + if isinstance( + new_e, (OSError, NewConnectionError, TimeoutError, SSLError) + ) and (conn and conn.proxy and not conn.has_connected_to_proxy): + new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) + raise new_e +  + # conn.request() calls http.client.*.request, not the method in + # urllib3.request. It also calls makefile (recv) on the socket. + try: + conn.request( + method, + url, + body=body, + headers=headers, + chunked=chunked, + preload_content=preload_content, + decode_content=decode_content, + enforce_content_length=enforce_content_length, + ) +  + # We are swallowing BrokenPipeError (errno.EPIPE) since the server is + # legitimately able to close the connection after sending a valid response. + # With this behaviour, the received response is still readable. + except BrokenPipeError: + pass + except OSError as e: + # MacOS/Linux + # EPROTOTYPE and ECONNRESET are needed on macOS + # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ + # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. + if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: + raise +  + # Reset the timeout for the recv() on the socket + read_timeout = timeout_obj.read_timeout +  + if not conn.is_closed: + # In Python 3 socket.py will catch EAGAIN and return None when you + # try and read into the file pointer created by http.client, which + # instead raises a BadStatusLine exception. Instead of catching + # the exception and assuming all BadStatusLine exceptions are read + # timeouts, check for a zero timeout before making the request. + if read_timeout == 0: + raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={read_timeout})" + ) + conn.timeout = read_timeout +  + # Receive the response from the server + try: +> response = conn.getresponse() + ^^^^^^^^^^^^^^^^^^ + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse + httplib_response = super().getresponse() + ^^^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:1428: in getresponse + response.begin() +/usr/lib/python3.12/http/client.py:331: in begin + version, status, reason = self._read_status() + ^^^^^^^^^^^^^^^^^^^ +/usr/lib/python3.12/http/client.py:292: in _read_status + line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +b = + + def readinto(self, b): +  """Read up to len(b) bytes into the writable buffer *b* and return +  the number of bytes read. If the socket is non-blocking and no bytes +  are available, None is returned. +  +  If *b* is non-empty, a 0 return value indicates that the connection +  was shutdown at the other end. +  """ + self._checkClosed() + self._checkReadable() + if self._timeout_occurred: + raise OSError("cannot read from timed out object") + while True: + try: +> return self._sock.recv_into(b) + ^^^^^^^^^^^^^^^^^^^^^^^ +E TimeoutError: timed out + +/usr/lib/python3.12/socket.py:707: TimeoutError + +The above exception was the direct cause of the following exception: + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: +> resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen + retries = retries.increment( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment + raise reraise(type(error), error, _stacktrace) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise + raise value +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen + response = self._make_request( +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request + self._raise_timeout(err=e, url=url, timeout_value=read_timeout) +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_position' +timeout_value = 120 + + def _raise_timeout( + self, + err: BaseSSLError | OSError | SocketTimeout, + url: str, + timeout_value: _TYPE_TIMEOUT | None, + ) -> None: +  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" +  + if isinstance(err, SocketTimeout): +> raise ReadTimeoutError( + self, url, f"Read timed out. (read timeout={timeout_value})" + ) from err +E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=58727): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError + +During handling of the above exception, another exception occurred: + +method = )> +url = 'http://0.0.0.0:58727/graspable/pick_up_object_by_position' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: +> resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + +src/python/arcor2_web/rest.py:259: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put + return request("put", url, data=data, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request + return session.request(method=method, url=url, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request + resp = self.send(prep, **send_kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send + r = adapter.send(request, **kwargs) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +self = +request = , stream = False +timeout = Timeout(connect=3.05, read=120, total=None), verify = True +cert = None, proxies = OrderedDict() + + def send( + self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None + ): +  """Sends PreparedRequest object. Returns Response object. +  +  :param request: The :class:`PreparedRequest ` being sent. +  :param stream: (optional) Whether to stream the request content. +  :param timeout: (optional) How long to wait for the server to send +  data before giving up, as a float, or a :ref:`(connect timeout, +  read timeout) ` tuple. +  :type timeout: float or tuple or urllib3 Timeout object +  :param verify: (optional) Either a boolean, in which case it controls whether +  we verify the server's TLS certificate, or a string, in which case it +  must be a path to a CA bundle to use +  :param cert: (optional) Any user-provided SSL certificate to be trusted. +  :param proxies: (optional) The proxies dictionary to apply to the request. +  :rtype: requests.Response +  """ +  + try: + conn = self.get_connection_with_tls_context( + request, verify, proxies=proxies, cert=cert + ) + except LocationValueError as e: + raise InvalidURL(e, request=request) +  + self.cert_verify(conn, request.url, verify, cert) + url = self.request_url(request, proxies) + self.add_headers( + request, + stream=stream, + timeout=timeout, + verify=verify, + cert=cert, + proxies=proxies, + ) +  + chunked = not (request.body is None or "Content-Length" in request.headers) +  + if isinstance(timeout, tuple): + try: + connect, read = timeout + timeout = TimeoutSauce(connect=connect, read=read) + except ValueError: + raise ValueError( + f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " + f"or a single float to set both timeouts to the same value." + ) + elif isinstance(timeout, TimeoutSauce): + pass + else: + timeout = TimeoutSauce(connect=timeout, read=timeout) +  + try: + resp = conn.urlopen( + method=request.method, + url=url, + body=request.body, + headers=request.headers, + redirect=False, + assert_same_host=False, + preload_content=False, + decode_content=False, + retries=self.max_retries, + timeout=timeout, + chunked=chunked, + ) +  + except (ProtocolError, OSError) as err: + raise ConnectionError(err, request=request) +  + except MaxRetryError as e: + if isinstance(e.reason, ConnectTimeoutError): + # TODO: Remove this in 3.0.0: see #2811 + if not isinstance(e.reason, NewConnectionError): + raise ConnectTimeout(e, request=request) +  + if isinstance(e.reason, ResponseError): + raise RetryError(e, request=request) +  + if isinstance(e.reason, _ProxyError): + raise ProxyError(e, request=request) +  + if isinstance(e.reason, _SSLError): + # This branch is for urllib3 v1.22 and later. + raise SSLError(e, request=request) +  + raise ConnectionError(e, request=request) +  + except ClosedPoolError as e: + raise ConnectionError(e, request=request) +  + except _ProxyError as e: + raise ProxyError(e) +  + except (_SSLError, _HTTPError) as e: + if isinstance(e, _SSLError): + # This branch is for urllib3 versions earlier than v1.22 + raise SSLError(e, request=request) + elif isinstance(e, ReadTimeoutError): +> raise ReadTimeout(e, request=request) +E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=58727): Read timed out. (read timeout=120) + +/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout + +The above exception was the direct cause of the following exception: + +start_processes = Urls(ros_domain_id='220', robot_url='http://0.0.0.0:58727', storage_url='http://127.0.0.1:41809') + + @pytest.mark.timeout(321) + def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() +  + X = 0.0 + Y = 0.5 + Z = 0.1 +  + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 +  + object = Cylinder("Cyl1", 0.1, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) +  +> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:30: +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ +src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position + rest.call( +_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ + +method = )> +url = 'http://0.0.0.0:58727/graspable/pick_up_object_by_position' + + def call( + method: Method, + url: str, + *, + return_type: ReturnType = None, + list_return_type: ReturnType = None, + body: OptBody = None, + params: OptParams = None, + raw_params: bool = False, + files: OptFiles = None, + timeout: OptTimeout = None, + ) -> ReturnValue: +  """Universal function for calling REST APIs. +  +  :param method: HTTP method. +  :param url: Resource address. +  :param return_type: If set, function will try to return one value of a given type. +  :param list_return_type: If set, function will try to return list of a given type. +  :param body: Data to be send in the request body. +  :param params: Path parameters. +  :param raw_params: Parameters are by default converted to camelCase form. +  If you do not want this behavior, pass True here +  :param files: Instead of body, it is possible to send files. +  :param timeout: Specific timeout for a call. +  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. +  """ + logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") +  + if body and files: + raise RestException("Can't send data and files at the same time.") +  + if return_type and list_return_type: + raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") +  + if return_type is None: + return_type = list_return_type +  + d: list[Any] | dict[str, Any] | None = None +  + # prepare body data into dict or list (if any) + if isinstance(body, JsonSchemaMixin): + d = humps.camelize(body.to_dict()) + elif isinstance(body, list): + d = [] + for dd in body: + if isinstance(dd, JsonSchemaMixin): + d.append(humps.camelize(dd.to_dict())) + else: + d.append(dd) + elif body is not None: + raise RestException("Unsupported type of data.") +  + if params and not raw_params: + params = humps.camelize(params) +  + # requests just simply stringifies parameters, which does not work for booleans + for param_name, param_value in params.items(): + if isinstance(param_value, bool): + params[param_name] = "true" if param_value else "false" +  + if timeout is None: + timeout = Timeout() +  + try: + if files: + files = humps.camelize(files) + resp = method.value(url, files=files, timeout=timeout, params=params) + else: + resp = method.value( + url, + data=json.dumps(d).encode("utf-8") if d is not None else None, + timeout=timeout, + headers=headers, + params=params, + ) + except requests.exceptions.RequestException as e: + logger.debug("Request failed.", exc_info=True) + # TODO would be good to provide more meaningful message but the original one could be very very long +> raise RestException("Catastrophic system error.") from e +E arcor2_web.rest.RestException: Catastrophic system error. + +src/python/arcor2_web/rest.py:269: RestException +---------------------------- Captured log teardown ----------------------------- +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-42-39-321904-DESKTOP-HG3Q5KV-11687 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [11697] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [11698] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [11699] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [11700] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [11701] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409759.572370442] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.575114270] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.577347037] [controller_manager]: Subscribing to '/robot_description' topic for robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.579718092] [controller_manager]: update rate is 500 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.579745054] [controller_manager]: Overruns handling is : enabled +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.579750444] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409759.579841808] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.587985256] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.590568079] [controller_manager]: Loading hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.590936359] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.591006081] [controller_manager]: Initialize hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.597676025] [controller_manager]: Successful initialization of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598778450] [resource_manager]: 'configure' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598806593] [resource_manager]: Successful 'configure' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598826561] [resource_manager]: 'activate' hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598830839] [resource_manager]: Successful 'activate' of hardware 'ur5e' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598875915] [controller_manager]: Registering statistics for : ur5e +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598906874] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409759.796350801] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.048086102] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.048137820] [controller_manager]: Loading controller 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.050369014] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.063311131] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.064269262] [controller_manager]: Configuring controller: 'joint_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.064390081] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.071389411] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.072355622] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.075025881] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.075615661] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.075650738] [controller_manager]: Loading controller 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.079958879] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.090902207] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.091127927] [controller_manager]: Configuring controller: 'io_and_status_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.092898246] [controller_manager]: Activating controllers: [ io_and_status_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409760.102387266] [controller_manager]: Overrun might occur, Total time : 8218.786 us (Expected < 2000.000 us) --> Read time : 12.393 us, Update time : 8204.279 us (Switch time : 8164.122 us (Switch chained mode time : 0.221 us, perform mode change time : 9.688 us, Activation time : 8137.792 us, Deactivation time : 0.080 us)), Write time : 2.114 us +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409760.102436369] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.361093 ms (missed cycles : 5). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.102400562] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.104897746] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.105534206] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.105589781] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.105927302] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.113736721] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.114973832] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.115183214] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.115243960] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.116900007] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.118259791] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.121255458] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.121986862] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.122016658] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.122926768] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.136838878] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.137019340] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.138912768] [force_torque_sensor_broadcaster]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.140936653] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.142264922] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.144992661] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.145726586] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.145756282] [controller_manager]: Loading controller 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.146024652] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.157190588] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.157438333] [controller_manager]: Configuring controller: 'ur_configuration_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.160904966] [controller_manager]: Activating controllers: [ ur_configuration_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.162301847] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.165009810] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.165995709] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.166026167] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.166321508] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.175373999] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.187133253] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187351883] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187471952] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187490887] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187507800] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.189338703] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.196883635] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.198482022] [controller_manager]: Successfully switched controllers! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.201254175] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.300896948] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 11700] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.552925686] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.552971624] [controller_manager]: Loading controller 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.553297012] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.577285509] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578372249] [controller_manager]: Configuring controller: 'joint_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578501324] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578534357] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578549736] [joint_trajectory_controller]: Using 'splines' interpolation method. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.579896739] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.585454410] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.585485018] [controller_manager]: Loading controller 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.586866323] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.599101881] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.599337505] [controller_manager]: Configuring controller: 'forward_velocity_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.599933978] [forward_velocity_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.601466960] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.601501486] [controller_manager]: Loading controller 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.602178979] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.615091618] [spawner_joint_trajectory_controller]: Loaded forward_position_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.615361431] [controller_manager]: Configuring controller: 'forward_position_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.615856091] [forward_position_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.617339460] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.617374387] [controller_manager]: Loading controller 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.617963511] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.631015802] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.631256749] [controller_manager]: Configuring controller: 'forward_effort_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.631834047] [forward_effort_controller]: configure successful +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.633593280] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.633629018] [controller_manager]: Loading controller 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.633921754] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.647113599] [spawner_joint_trajectory_controller]: Loaded force_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.647375633] [controller_manager]: Configuring controller: 'force_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.657399630] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.657454774] [controller_manager]: Loading controller 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.657766216] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.671224127] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.671490659] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.677654796] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.677686366] [controller_manager]: Loading controller 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.677956966] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.691288188] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.691542766] [controller_manager]: Configuring controller: 'freedrive_mode_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.693839549] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.693868144] [controller_manager]: Loading controller 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.694081689] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.707342912] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.707635995] [controller_manager]: Configuring controller: 'tool_contact_controller' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 11701] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409762.154008266] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 11698] +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409762.441361682] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409762.441407409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409764.366208387] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.132930 ms (missed cycles : 4). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409765.470213376] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.138190 ms (missed cycles : 3). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409767.370228047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.152900 ms (missed cycles : 2). +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409767.764106413] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409767.764159974] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409768.764221564] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409768.764270436] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409769.764353752] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409769.764403917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409770.764279850] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409770.764331438] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409771.764296829] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409771.764348337] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409772.764151065] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409772.764195329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409773.764308030] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409773.764363706] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409774.764317523] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409774.764376144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409775.764264462] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409775.764315809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409776.764271604] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409776.764333021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409777.764201295] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409777.764256730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409778.764213293] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409778.764258990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409779.764320319] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409779.764402566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409780.764376270] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409780.764428580] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409781.764234109] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409781.764284775] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409782.764221240] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409782.764277897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409783.764200595] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409783.764253175] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409784.764206302] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409784.764260665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409785.764284875] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409785.764335051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409786.764236769] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409786.764274701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409787.764304257] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409787.764419431] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409788.764223979] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409788.764272491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409789.764202853] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409789.764256505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409790.764193047] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409790.764256858] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409791.764231436] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409791.764286090] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409792.560261504] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409792.764167109] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409792.764212796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409793.111256952] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409793.764242579] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409793.764315818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409794.664282440] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409794.664339488] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409794.764221255] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409794.764293623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409795.764231286] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409795.764283425] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409796.219305492] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409796.219378792] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409796.764140746] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409796.764188406] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409797.686221356] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409797.764151306] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409797.764194769] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409798.764168300] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409798.764231409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409799.764138819] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409799.764192160] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409800.764216317] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409800.764271802] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409801.764173245] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409801.764225634] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409802.735369275] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409802.764235980] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409802.764284542] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409803.286375253] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409803.764228797] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409803.764289833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409804.202246564] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409804.202289115] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409804.764350223] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409804.764403744] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409805.764143247] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409805.764191188] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409806.764178860] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409806.764253532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409807.764154195] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409807.764204591] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409808.764180726] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409808.764230901] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409809.764191153] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409809.764250566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409810.764180819] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409810.764249470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409811.764191596] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409811.764238565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409812.764181212] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409812.764227530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409813.764191675] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409813.764249625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409814.764181026] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409814.764229659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409815.764183111] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409815.764235070] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409816.764308311] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409816.764362654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409816.782527509] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409817.333485734] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409817.764216117] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409817.764269418] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409818.764184825] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409818.764234409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409819.267339971] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409819.267390677] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409819.764203527] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409819.764260024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.606993302] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.607043196] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.748194354] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.764149736] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409820.764189692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409821.764104533] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409821.764161561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409822.764151759] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409822.764223735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409823.764083220] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409823.764136411] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409824.764143506] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409824.764198079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409824.766423494] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409825.317389150] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409825.764192539] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409825.764233758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409826.764187190] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409826.764257223] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409827.764097767] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409827.764145818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409828.024658497] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409828.024714844] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409828.764153389] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409828.764204746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409829.764194344] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409829.764249919] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409830.764183563] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409830.764231384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409831.764209764] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409831.764259027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409832.764289690] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409832.764339715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409833.764213431] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409833.764263245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409834.764203131] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409834.764252324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409835.764172697] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409835.764224516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409836.764150921] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409836.764198401] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409837.764182699] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409837.764239306] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409838.340023718] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409838.764146377] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409838.764190791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409838.891018373] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409839.764202651] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409839.764240242] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409840.083369699] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.083423341] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.764216716] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409840.764281318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.871145575] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.871208274] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409841.764198548] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409841.764251038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409842.668214952] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409842.764281857] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409842.764335960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409843.764142655] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409843.764192810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409844.764157054] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409844.764230965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409845.764392813] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409845.764455562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409846.764194145] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409846.764248037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409847.764208528] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409847.764249636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409848.764175566] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409848.764228657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409849.533039064] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409849.764145599] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409849.764197628] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409850.083961123] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409850.764292220] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409850.764342245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409850.864032152] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409850.864078340] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409851.764195297] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409851.764274027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409852.764175676] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409852.764232103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409853.764172091] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409853.764220353] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409854.764177934] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409854.764228691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409855.764216244] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409855.764271629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409856.764091563] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409856.764138863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409857.764163193] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409857.764211815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409858.764203304] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409858.764254541] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409859.764161922] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409859.764208641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409860.764203237] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409860.764255276] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409861.069795227] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409861.620866347] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409861.764215221] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409861.764266990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409862.438528062] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.438587956] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.764335855] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409862.764378456] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.767017921] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.767074939] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.962206344] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409863.764169838] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409863.764219502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409864.764191833] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409864.764242560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409865.764171424] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409865.764229274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409866.764185664] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409866.764240809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409867.764406392] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409867.764454714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409867.960177944] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409868.511163799] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409868.764178218] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409868.764236378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409869.255438544] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409869.255499320] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409869.764144008] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409869.764197760] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409870.764189251] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409870.764241730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409871.764179272] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409871.764257400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409872.764179042] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409872.764254194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409873.764170001] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409873.764238240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409874.764203882] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409874.764273675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409875.764251384] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409875.764307981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409876.764219542] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409876.764274557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409877.764198201] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409877.764267423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409878.764162437] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409878.764212001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409879.764204727] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409879.764260693] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409879.948926906] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409880.499862929] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409880.764196880] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409880.764254880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409881.764169136] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409881.764239410] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409881.983318541] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409882.534276948] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409882.764203731] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409882.764260399] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409883.387738055] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.387793610] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.551063022] [scaled_joint_trajectory_controller]: Received new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.551120781] [scaled_joint_trajectory_controller]: Accepted new action goal +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.760222550] [scaled_joint_trajectory_controller]: Goal reached, success! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.764180751] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409883.764220707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409884.764142597] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409884.764195047] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409885.764155987] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409885.764202947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409886.764160882] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409886.764204946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409887.764237674] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409887.764295594] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409888.764190966] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409888.764249117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409889.363671580] [io_and_status_controller]: Setting speed slider to 50.00%. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409889.764147673] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409889.764195784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409889.914686842] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409890.764151902] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409890.764221915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409890.828126416] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409890.828182784] [io_and_status_controller]: Payload has been set successfully +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409891.764203500] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409891.764251812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409892.764138920] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409892.764187272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409893.764178169] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409893.764250006] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409894.764175368] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409894.764223350] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409895.764221848] [controller_manager]: Received robot description from topic. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409895.764274678] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409762.319032737] [robot_state_publisher]: Robot initialized +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409911.373843058] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:47 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:58727 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:58727 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "GET /healthz/ready HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "PUT /system/start HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "GET /system/running HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "GET /state/started HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.257718930] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize rclcpp +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.257775468] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node parameters +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.257788693] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node and executor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.264980653] [moveit_1502777475.moveit.py.cpp_initializer]: Spin separate thread +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.266709778] [moveit_1502777475.moveit.ros.rdf_loader]: Loaded robot model in 0.00161847 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.266735237] [moveit_1502777475.moveit.core.robot_model]: Loading robot model 'ur5e'... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.266743102] [moveit_1502777475.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409772.279139666] [moveit_1502777475.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.281573511] [moveit_1502777475.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.334319664] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.334435014] [moveit_1502777475.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.335323071] [moveit_1502777475.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.335873668] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.335886742] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.336014686] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.336392794] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.336480110] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting planning scene monitor +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337085761] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337097563] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337542359] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337993165] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409772.338242710] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409772.338258289] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.820442676] [moveit_1502777475.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.821083354] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822069167] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822079546] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822369427] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822385167] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822410565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822416186] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822426466] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.823013311] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.825261203] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.825276572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.827036275] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.827049400] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.827853978] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.856359461] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.859791132] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.859954893] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.859977827] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.860833573] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:55 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:55 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:43:09] "PUT /state/start HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:43:09] "GET /joints HTTP/1.1" 200 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:43:10] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:10 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:10 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:11 INFO  Generated 20 grasp options for object Cyl1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  IK accepted: 20 grasp options. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:14 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:14 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:15 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:15 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153443391] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153563640] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153577316] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153595170] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409796.153662067] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.154085602] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.154206211] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.169621863] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.170072870] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.170416763] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216943760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216965231] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216983004] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216989677] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.217007110] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218558112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218597668] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218622855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218775075] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.219582153] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.219600488] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409797.720152829] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409797.724501172] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:18 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:18 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:19 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:19 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:20 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:20 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:25 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430164123] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430238374] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430252491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430265947] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409805.430344977] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430586536] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430635269] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.433369314] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.433788250] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.433897812] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.434094120] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409815.434251880] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003569 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.442111916] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.442221494] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:35 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:35 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:36 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:36 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:38 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:38 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584147221] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584203178] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584211964] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584223005] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409820.584299541] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584518948] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584598829] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.597357623] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.597428688] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.597522907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604442585] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604467592] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604485016] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604493532] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604512668] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606562112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606591648] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606612728] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606725563] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.607298807] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.607312493] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.757777293] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.762481005] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:41 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:41 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:48 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309517858] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309572281] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309580797] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309592019] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409828.309701827] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309951271] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.310044769] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.313605646] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.313763095] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.314096699] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.314217483] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409838.314339981] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004222 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.335261686] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.335320918] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:59 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:59 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.334937564] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.334986878] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.334994031] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.335004962] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409840.335072430] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.335303730] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.335344007] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.361554961] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.362156544] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.362570550] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868328733] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868368077] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868408855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868422000] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868454421] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870634349] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870682330] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870704953] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870838467] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.871434003] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.871448691] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409842.671872612] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409842.676467122] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:07 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:07 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:08 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:08 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:09 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:09 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:11 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:11 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:11 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005531569] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005608716] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005620558] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005633563] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409851.005712773] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005958300] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.006008856] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008170904] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008356352] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008610689] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008726895] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409861.008897064] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.002831 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.027596805] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.027655586] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724713760] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724773343] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724782410] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724793952] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409862.724870548] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.725097599] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.725148887] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.741512678] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.741581709] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.741639619] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765078426] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765101149] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765115166] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765122760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765139312] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766605713] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766639918] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766661479] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766773902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.767293940] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.767308498] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.967715412] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.972475057] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:25 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:25 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:26 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:26 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:27 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:27 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:28 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:28 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:29 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:29 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:29 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903117283] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903168130] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903175123] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903184341] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409869.903280754] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903521070] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903615410] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.906350239] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.906886684] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.907037235] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.907146348] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409879.907300711] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003617 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.930359365] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.930435400] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:39 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:39 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:43 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544616182] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544667620] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544674794] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544684552] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409883.544758092] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544983991] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.545065927] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.548860186] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.548946149] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549026222] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:43 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549221883] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549240869] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549254145] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549261188] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549276377] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550556860] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550584162] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550612315] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550723767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.551306820] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.551322309] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.801675456] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.806462571] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:44 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:44 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:45 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:45 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:48 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:48 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:49 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:49 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:50 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:50 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:51 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:51 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:51 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023659043] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023709839] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023716782] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023726441] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409891.023790152] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.024005240] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.024046759] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.026011506] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.027183142] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.027674426] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.027763870] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409901.027877612] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003787 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.030747966] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.030807449] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478695882] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478746709] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478753822] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478763250] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409902.478825398] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.479049083] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.479089971] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.491502647] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.491578932] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.491652091] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Executing plan +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658462590] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658489952] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658507485] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658516202] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658536400] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660578857] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660618292] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660643460] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660760553] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.661270336] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.661281607] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.861545943] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.866470690] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:03 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:03 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:04 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:04 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:05 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:05 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:06 INFO  Robot mode: 7 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:06 INFO  Program running: True +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:07 INFO  Planning trajectory +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578555476] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578604820] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578613286] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578622273] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409907.578684071] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578887607] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578929567] [moveit_py]: Calling Planner 'OMPL' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409911.502946530] [rclcpp]: signal_handler(signum=15) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.582781701] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.582860536] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.583501359] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.583585333] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409917.583696751] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004731 seconds +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.604220980] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.604280282] [moveit_py]: Planner 'OMPL' failed with error code FAILURE +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:43 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:43 ERROR  ROS worker command failed. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 conn.send({"status": "ok", "result": result}) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send_bytes(_ForkingPickler.dumps(obj)) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send(header + buf) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 n = write(self._handle, buf) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 BrokenPipeError: [Errno 32] Broken pipe +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:58 ERROR  Failed to switch controllers off during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:58 ERROR  Failed to clear freedrive mode during shutdown. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:58 ERROR  Failed to shutdown rclpy. +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409958.792785146] [moveit_1502777475.moveit.ros.moveit_cpp]: Deleting MoveItCpp +=============================== warnings summary =============================== +src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:13 + src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html + @pytest.mark.timeout(321) + +-- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html +- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_cylinder_by_position.py@tests.xml - +=========================== short test summary info ============================ +FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py::test_pick_and_place_cylinder_by_position[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. +=================== 1 failed, 1 warning in 202.00s (0:03:21) =================== + + + +✓ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py:../tests succeeded in 34.46s. ++ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests succeeded after 3 attempts in 35.13s. +✓ src/python/arcor2_ur/tests/test_interaction.py succeeded in 58.78s (cached locally). +✓ src/python/arcor2_ur/tests/test_ur.py succeeded in 3.44s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests failed after 5 attempts in 100.82s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests failed after 5 attempts in 100.14s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests failed after 5 attempts in 204.06s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests failed after 5 attempts in 202.80s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests failed after 5 attempts in 49.36s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests failed after 5 attempts in 82.90s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests failed after 5 attempts in 142.73s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests failed after 5 attempts in 204.15s. +✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests failed after 5 attempts in 177.11s. +✕ src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests failed after 5 attempts in 44.37s. +✕ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests failed after 5 attempts in 44.09s. +✕ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests failed after 5 attempts in 46.75s. +✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests failed after 5 attempts in 45.30s. +✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests failed after 5 attempts in 51.39s. +✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests failed after 5 attempts in 70.44s. +✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests failed after 5 attempts in 50.15s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests failed after 5 attempts in 34.25s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests failed after 5 attempts in 33.75s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests failed after 5 attempts in 31.82s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests failed after 5 attempts in 33.69s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests failed after 5 attempts in 33.48s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests failed after 5 attempts in 33.14s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests failed after 5 attempts in 33.63s. +✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests failed after 5 attempts in 34.05s. +✕ src/python/arcor2_ur/tests/test_suck_effector.py failed after 5 attempts in 10.84s. diff --git a/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py new file mode 100644 index 00000000..b3770641 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py @@ -0,0 +1,57 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box, Cylinder, Sphere +from arcor2_object_types.abstract import GraspableState +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_graspable(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box", 0.2, 0.2, 0.95) + scene_service.upsert_graspable(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl", 0.1, 0.95) + scene_service.upsert_graspable( + cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), GraspableState.RESERVED + ) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_graspable( + sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1)), GraspableState.HIDDEN + ) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() + + box = Box("Box1", 0.1, 0.1, 0.2) diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py new file mode 100644 index 00000000..347accb8 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py @@ -0,0 +1,55 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_overhead_box(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box2", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box3", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box4", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py new file mode 100644 index 00000000..86cf082e --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py @@ -0,0 +1,55 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_overhead_cylinder(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl3", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl4", 0.2, 0.2) + scene_service.upsert_collision(cyl, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py new file mode 100644 index 00000000..99ea5484 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py @@ -0,0 +1,70 @@ +import time +from pathlib import Path + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Mesh +from arcor2_scene_data import scene_service +from arcor2_storage import client as storage_client +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + +MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl" +MESH_ASSET_ID = "triangle_block.stl" + + +@pytest.mark.timeout(400) +def test_overhead_mesh(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + + storage_client.URL = start_processes.storage_url + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) + + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.6 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + + ot.move_to_pose("", start_pose, 0.3, safe=False) + + mesh = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_collision(mesh, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert mesh.id in scene_service.collision_ids() + time.sleep(1) + + mesh = Mesh("Mesh2", MESH_ASSET_ID) + scene_service.upsert_collision(mesh, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert mesh.id in scene_service.collision_ids() + time.sleep(1) + + mesh = Mesh("Mesh3", MESH_ASSET_ID) + scene_service.upsert_collision(mesh, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert mesh.id in scene_service.collision_ids() + time.sleep(1) + + mesh = Mesh("Mesh4", MESH_ASSET_ID) + scene_service.upsert_collision(mesh, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert mesh.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py new file mode 100644 index 00000000..6a016848 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py @@ -0,0 +1,55 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_overhead_sphere(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.6 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere2", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere3", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere4", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py new file mode 100644 index 00000000..1a45c9ff --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py @@ -0,0 +1,45 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_side_box(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + box = Box("Box1", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box2", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py new file mode 100644 index 00000000..3846c25f --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py @@ -0,0 +1,45 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_side_cylinder(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + cyl = Cylinder("Cyl1", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + cyl = Cylinder("Cyl2", 0.1, 0.95) + scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert cyl.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_mesh.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py similarity index 53% rename from src/python/arcor2_ur/tests/test_mesh.py rename to src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py index 5dde7281..29dd639d 100644 --- a/src/python/arcor2_ur/tests/test_mesh.py +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py @@ -10,16 +10,20 @@ from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls -MESH_PATH = Path(__file__).parent / "test_mesh_object" / "taper.stl" -MESH_ASSET_ID = "taper.stl" +MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl" +MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(321) -def test_mesh_collision_avoidance(start_processes: Urls) -> None: +@pytest.mark.timeout(400) +def test_side_mesh(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + storage_client.URL = start_processes.storage_url + storage_client.create_asset( - MESH_ASSET_ID, MESH_PATH.read_bytes(), description="Test mesh for UR5e collision avoidance." + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", ) scene_service.URL = start_processes.robot_url @@ -37,15 +41,30 @@ def test_mesh_collision_avoidance(start_processes: Urls) -> None: goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) ot.move_to_pose("", start_pose, 0.3, safe=False) + time.sleep(1) + + mesh_positions = [ + # rgith + Position(0.5, 0.5, 0.25), + Position(0.5, 0.5, 0.55), + # left + Position(-0.5, 0.5, 0.25), + Position(-0.5, 0.5, 0.55), + ] + + for idx, position in enumerate(mesh_positions, start=1): + mesh = Mesh(f"Mesh{idx}", MESH_ASSET_ID) + + scene_service.upsert_collision( + mesh, + Pose(position, Orientation(0, 0, 0, 1)), + ) - mesh = Mesh("Mesh1", MESH_ASSET_ID) - mesh_pose = Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)) + assert mesh.id in scene_service.collision_ids() + time.sleep(3) - scene_service.upsert_collision(mesh, mesh_pose) - assert mesh.id in scene_service.collision_ids() - time.sleep(100) + ot.move_to_pose("", goal_pose, 0.3) - ot.move_to_pose("", goal_pose, 0.3, safe=True) reached = ot.get_end_effector_pose("") assert goal_pose.position.distance(reached.position) < 0.03 diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py new file mode 100644 index 00000000..03851788 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py @@ -0,0 +1,65 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_side_sphere(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.5 + Y = 0.0 + Z = 0.3 + + start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) + goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) + ot.move_to_pose("", start_pose, 0.3, safe=False) + + sphere = Sphere("Sphere1", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere2", 0.1) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere3", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere4", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere5", 0.1) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + sphere = Sphere("Sphere6", 0.09) + scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert sphere.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", goal_pose, 0.3) + + reached = ot.get_end_effector_pose("") + assert goal_pose.position.distance(reached.position) < 0.03 + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_graspable_collision_object.py b/src/python/arcor2_ur/tests/test_graspable_collision_object.py deleted file mode 100644 index af23e48e..00000000 --- a/src/python/arcor2_ur/tests/test_graspable_collision_object.py +++ /dev/null @@ -1,59 +0,0 @@ -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Cylinder -from arcor2_object_types.abstract import GraspableSource, GraspableState -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -@pytest.mark.timeout(321) -def test_graspable_collision_object(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - y = 0.0 - z = 0.2 - x = 0.9 - - start_pose = Pose(Position(x, y, z), Orientation(0, 0, 0, 1)) - goal_pose = Pose(Position(y, x, z), Orientation(0, 0, 0, 1)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - cyl1 = Cylinder("Cyl1", 0.1, 0.95) - scene_service.upsert_graspable( - cyl1, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1)), state=GraspableState.WORLD - ) - assert cyl1.id in scene_service.collision_ids() - time.sleep(1) - - cyl2 = Cylinder("Cyl2", 0.1, 0.95) - scene_service.upsert_collision(cyl2, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl2.id in scene_service.collision_ids() - time.sleep(1) - - cyl3 = Cylinder("Cyl3", 0.1, 0.95) - scene_service.upsert_graspable( - cyl3, - Pose(Position(0.0, 0.0, 0.0), Orientation(0, 0, 0, 1)), - state=GraspableState.ATTACHED, - source=GraspableSource.OTHER, - ) - assert cyl3.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_mesh_object/taper.stl b/src/python/arcor2_ur/tests/test_mesh_object/taper.stl deleted file mode 100644 index 3c94a3d0..00000000 --- a/src/python/arcor2_ur/tests/test_mesh_object/taper.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2faa852002f4d06edc18faf5f9de3753e5e46d9349f75acea44812ac25b28a2c -size 249334 diff --git a/src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl b/src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl new file mode 100644 index 00000000..4447107a --- /dev/null +++ b/src/python/arcor2_ur/tests/test_mesh_object/triangle_block.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f716942e9d3e23090ab22378eae5f238cbe19329172af17c32a25bbe52221e47 +size 484 diff --git a/src/python/arcor2_ur/tests/test_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_overhead_obstacle.py deleted file mode 100644 index f0e1ca28..00000000 --- a/src/python/arcor2_ur/tests/test_overhead_obstacle.py +++ /dev/null @@ -1,147 +0,0 @@ -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Box, Cylinder, Sphere -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -@pytest.mark.timeout(321) -def test_box(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - X = 0.5 - Y = 0.0 - Z = 0.3 - - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - box = Box("Box1", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box2", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box3", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box4", 0.2, 0.2, 0.2) - scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() - - -@pytest.mark.timeout(321) -def test_cylinder(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - X = 0.5 - Y = 0.0 - Z = 0.3 - - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - cyl = Cylinder("Cyl1", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl2", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl3", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl4", 0.2, 0.2) - scene_service.upsert_collision(cyl, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() - - -@pytest.mark.timeout(321) -def test_sphere(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - X = 0.5 - Y = 0.0 - Z = 0.3 - - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - sphere = Sphere("Sphere1", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere2", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere3", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere4", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py new file mode 100644 index 00000000..523fdc59 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_pick_and_place_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py new file mode 100644 index 00000000..fcd147de --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_and_place_box_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py new file mode 100644 index 00000000..6d180fba --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Cylinder("Cyl1", 0.1, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py new file mode 100644 index 00000000..2a5d4559 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Cylinder("Cyl1", 0.1, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py new file mode 100644 index 00000000..584efaa5 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py @@ -0,0 +1,55 @@ +import time +from pathlib import Path + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Mesh +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_storage import client as storage_client +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + +MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl" +MESH_ASSET_ID = "triangle_block.stl" + + +@pytest.mark.timeout(321) +def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + + storage_client.URL = start_processes.storage_url + + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) + + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py new file mode 100644 index 00000000..74df8d77 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py @@ -0,0 +1,54 @@ +import time +from pathlib import Path + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Mesh +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_storage import client as storage_client +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + +MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl" +MESH_ASSET_ID = "triangle_block.stl" + + +@pytest.mark.timeout(321) +def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + + storage_client.URL = start_processes.storage_url + + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) + + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py new file mode 100644 index 00000000..cbc78399 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_pick_and_place_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0.5, 0.5, 0.5, 0)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py new file mode 100644 index 00000000..7dc450a3 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Sphere("Sphere1", 0.1) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py new file mode 100644 index 00000000..01aa1258 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py @@ -0,0 +1,40 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Sphere("Sphere1", 0.1) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 + assert scene_service.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py new file mode 100644 index 00000000..abdb15e6 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py @@ -0,0 +1,35 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(400) +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_move_object.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py similarity index 61% rename from src/python/arcor2_ur/tests/test_move_object.py rename to src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py index 3d8ef39d..d2373802 100644 --- a/src/python/arcor2_ur/tests/test_move_object.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py @@ -10,9 +10,8 @@ from arcor2_ur.tests.conftest import Urls -# TODO: add asserts @pytest.mark.timeout(321) -def test_move_object(start_processes: Urls) -> None: +def test_pick_up_box_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() assert scene_service.started() @@ -20,13 +19,17 @@ def test_move_object(start_processes: Urls) -> None: ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 - box = Box("Box1", 0.1, 0.1, 0.2) - scene_service.upsert_graspable(box, Pose(Position(0, 0.5, 0.1), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object(Position(0, 0.5, 0.1), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) - ot.place_object(Pose(Position(0, -0.5, 0.1), Orientation(0, 0, 0, 1))) + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py new file mode 100644 index 00000000..5a999583 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py @@ -0,0 +1,35 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_up_cylinder_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Cylinder("Cyl1", 0.1, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py new file mode 100644 index 00000000..f526d044 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py @@ -0,0 +1,35 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Cylinder +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_up_cylinder_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Cylinder("Cyl1", 0.1, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py new file mode 100644 index 00000000..83e58e0d --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py @@ -0,0 +1,50 @@ +import time +from pathlib import Path + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Mesh +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_storage import client as storage_client +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + +MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl" +MESH_ASSET_ID = "triangle_block.stl" + + +@pytest.mark.timeout(321) +def test_pick_up_mesh_by_id(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + + storage_client.URL = start_processes.storage_url + + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) + + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py new file mode 100644 index 00000000..713b184b --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py @@ -0,0 +1,50 @@ +import time +from pathlib import Path + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Mesh +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_storage import client as storage_client +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + +MESH_PATH = Path(__file__).parents[1] / "test_mesh_object" / "triangle_block.stl" +MESH_ASSET_ID = "triangle_block.stl" + + +@pytest.mark.timeout(321) +def test_pick_up_mesh_by_position(start_processes: Urls) -> None: + assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" + + storage_client.URL = start_processes.storage_url + + storage_client.create_asset( + MESH_ASSET_ID, + MESH_PATH.read_bytes(), + description="Test mesh for UR5e collision avoidance.", + ) + + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Mesh("Mesh1", MESH_ASSET_ID) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py new file mode 100644 index 00000000..5119378b --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py @@ -0,0 +1,35 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_up_sphere_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Sphere("Sphere1", 0.1) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py new file mode 100644 index 00000000..a627420a --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py @@ -0,0 +1,35 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Sphere +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +@pytest.mark.timeout(321) +def test_pick_up_sphere_by_position(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Sphere("Sphere1", 0.1) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + + assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_side_obstacle.py b/src/python/arcor2_ur/tests/test_side_obstacle.py deleted file mode 100644 index ae23c0a9..00000000 --- a/src/python/arcor2_ur/tests/test_side_obstacle.py +++ /dev/null @@ -1,137 +0,0 @@ -import time - -import pytest - -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Box, Cylinder, Sphere -from arcor2_scene_data import scene_service -from arcor2_ur.object_types.ur5e import Ur5e, UrSettings -from arcor2_ur.tests.conftest import Urls - - -@pytest.mark.timeout(321) -def test_box(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - X = 0.5 - Y = 0.0 - Z = 0.3 - - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - box = Box("Box1", 0.2, 0.2, 0.95) - scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - box = Box("Box2", 0.2, 0.2, 0.95) - scene_service.upsert_collision(box, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert box.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() - - -@pytest.mark.timeout(321) -def test_cylinder(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - X = 0.5 - Y = 0.0 - Z = 0.3 - - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - cyl = Cylinder("Cyl1", 0.1, 0.95) - scene_service.upsert_collision(cyl, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - cyl = Cylinder("Cyl2", 0.1, 0.95) - scene_service.upsert_collision(cyl, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert cyl.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() - - -@pytest.mark.timeout(321) -def test_sphere(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() - - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 - - X = 0.5 - Y = 0.0 - Z = 0.3 - - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) - ot.move_to_pose("", start_pose, 0.3, safe=False) - - sphere = Sphere("Sphere1", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere2", 0.1) - scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere3", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere4", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.1), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere5", 0.1) - scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.3), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - sphere = Sphere("Sphere6", 0.09) - scene_service.upsert_collision(sphere, Pose(Position(-0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) - assert sphere.id in scene_service.collision_ids() - time.sleep(1) - - ot.move_to_pose("", goal_pose, 0.3) - - reached = ot.get_end_effector_pose("") - assert goal_pose.position.distance(reached.position) < 0.03 - - scene_service.delete_all_collisions() - ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_suck_effector.py b/src/python/arcor2_ur/tests/test_suck_effector.py index e45ea741..d3cbf831 100644 --- a/src/python/arcor2_ur/tests/test_suck_effector.py +++ b/src/python/arcor2_ur/tests/test_suck_effector.py @@ -1,7 +1,6 @@ import pytest -from arcor2.data.common import Orientation, Pose, Position -from arcor2.data.object_type import Box +from arcor2.data.common import Pose from arcor2_scene_data import scene_service from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.scripts.robot_publisher import load_robot_description @@ -20,8 +19,6 @@ def test_suck_effector(start_processes: Urls) -> None: assert "suction_cup_to_tcp" in robot_description scene_service.URL = start_processes.robot_url - box = Box("UniqueBoxId", 0.1, 0.1, 0.1) - scene_service.upsert_collision(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1))) scene_service.start() assert scene_service.started() From 483cf2d142af0adeec16f01939daee736a7dcb25 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Mon, 11 May 2026 21:01:04 +0200 Subject: [PATCH 09/11] fix(arcor2_ur): improve UR5e suction and collision tests --- .github/workflows/pants.yaml | 4 +- src/python/arcor2_object_types/abstract.py | 4 - src/python/arcor2_scene_data/scene_service.py | 14 +- src/python/arcor2_ur/common.py | 4 + src/python/arcor2_ur/data/moveit.yaml | 13 +- .../ur5e/collision/suction_cup_1cup.stl | 4 +- src/python/arcor2_ur/data/urdf/ur5e.srdf | 119 +- src/python/arcor2_ur/data/urdf/ur5e.urdf | 36 +- src/python/arcor2_ur/object_types/ur5e.py | 22 +- src/python/arcor2_ur/scripts/grasp_planner.py | 29 +- .../arcor2_ur/scripts/robot_publisher.py | 55 +- src/python/arcor2_ur/scripts/ros_worker.py | 155 +- src/python/arcor2_ur/scripts/ur.py | 36 +- src/python/arcor2_ur/tests/BUILD | 1 + src/python/arcor2_ur/tests/conftest.py | 109 +- .../arcor2_ur/tests/results_velka spatna.txt | 28886 ---------------- .../test_collision/test_obstacle_graspable.py | 3 - .../test_overhead_obstacle_box.py | 3 - .../test_overhead_obstacle_cylinder.py | 3 - .../test_overhead_obstacle_mesh.py | 3 - .../test_overhead_obstacle_sphere.py | 3 - .../test_collision/test_side_obstacle_box.py | 3 - .../test_side_obstacle_cylinder.py | 3 - .../test_collision/test_side_obstacle_mesh.py | 3 - .../test_side_obstacle_sphere.py | 3 - .../arcor2_ur/tests/test_interaction.py | 1 - .../test_pick_and_move_overhead_obstacle.py | 53 + .../test_pick_and_move_side_obstacle.py | 38 + .../test_pick_and_move_wall.py | 42 + .../test_pick_and_place_box_by_id.py | 11 +- .../test_pick_and_place_box_by_position.py | 11 +- .../test_pick_and_place_box_left.py | 37 + .../test_pick_and_place_cylinder_by_id.py | 11 +- ...est_pick_and_place_cylinder_by_position.py | 11 +- .../test_pick_and_place_mesh_by_id.py | 11 +- .../test_pick_and_place_mesh_by_position.py | 11 +- .../test_pick_and_place_rotated_box.py | 11 +- .../test_pick_and_place_sphere_by_id.py | 11 +- .../test_pick_and_place_sphere_by_position.py | 11 +- .../test_pick_up/test_pick_up_box_all.py | 32 + .../test_pick_up/test_pick_up_box_back.py | 32 + .../test_pick_up/test_pick_up_box_bottom.py | 32 + .../test_pick_up/test_pick_up_box_by_id.py | 7 +- .../test_pick_up_box_by_position.py | 7 +- .../test_pick_up/test_pick_up_box_front.py | 32 + .../test_pick_up/test_pick_up_box_left.py | 32 + .../test_pick_up/test_pick_up_box_right.py | 32 + .../test_pick_up_cylinder_by_id.py | 7 +- .../test_pick_up_cylinder_by_position.py | 7 +- .../test_pick_up/test_pick_up_mesh_by_id.py | 7 +- .../test_pick_up_mesh_by_position.py | 7 +- .../test_pick_up/test_pick_up_sphere_by_id.py | 7 +- .../test_pick_up_sphere_by_position.py | 7 +- .../arcor2_ur/tests/test_suck_effector.py | 3 - 54 files changed, 795 insertions(+), 29244 deletions(-) delete mode 100644 src/python/arcor2_ur/tests/results_velka spatna.txt create mode 100644 src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py create mode 100644 src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py create mode 100644 src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py diff --git a/.github/workflows/pants.yaml b/.github/workflows/pants.yaml index e9bbc43a..1a6980ae 100644 --- a/.github/workflows/pants.yaml +++ b/.github/workflows/pants.yaml @@ -82,10 +82,10 @@ jobs: source /opt/ros/jazzy/setup.bash pants --changed-since=origin/master --changed-dependents=transitive --tag='-unstable_ros' test - - name: Test changed unstable ROS tests with retries + - name: Test changed unstable ROS tests serially with retries run: | source /opt/ros/jazzy/setup.bash - pants --changed-since=origin/master --changed-dependents=transitive --tag='unstable_ros' test --test-attempts-default=5 + 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 :: diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index 7a7dcc0b..7a1fe199 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -213,16 +213,12 @@ class GraspableState(StrEnum): ATTACHED Attached to the robot end-effector. - - TOUCH_ALLOWED - Object acts as a collision obstacle, except for selected end-effector links. """ WORLD = "WORLD" RESERVED = "RESERVED" HIDDEN = "HIDDEN" ATTACHED = "ATTACHED" - TOUCH_ALLOWED = "TOUCH_ALLOWED" class GraspableSource(StrEnum): diff --git a/src/python/arcor2_scene_data/scene_service.py b/src/python/arcor2_scene_data/scene_service.py index a28466ea..f5928285 100644 --- a/src/python/arcor2_scene_data/scene_service.py +++ b/src/python/arcor2_scene_data/scene_service.py @@ -6,7 +6,7 @@ from dataclasses_jsonschema import JsonSchemaMixin -from arcor2.data.common import Pose, Position +from arcor2.data.common import Pose from arcor2.data.object_type import Model3dType, Models from arcor2.data.scene import LineCheck, LineCheckResult, MeshFocusAction from arcor2.exceptions import Arcor2Exception @@ -152,16 +152,6 @@ def upsert_graspable( rest.call(rest.Method.PUT, f"{URL}/collisions/{model_type}", body=body, params=params) -@handle(SceneServiceException, logger, message="Failed to get graspable object state.") -def graspable_state(object_id: str) -> GraspableState: - return GraspableState(rest.call(rest.Method.GET, f"{URL}/graspable/{object_id}/state", return_type=str)) - - -@handle(SceneServiceException, logger, message="Failed to get graspable object position.") -def graspable_position(object_id: str) -> Position: - return rest.call(rest.Method.GET, f"{URL}/graspable/{object_id}/position", return_type=Position) - - @handle(SceneServiceException, logger, message="Failed to delete the collision.") def delete_collision_id(collision_id: str) -> None: rest.call(rest.Method.DELETE, f"{URL}/collisions/{collision_id}") @@ -245,8 +235,6 @@ def world_pose(transform_id: str) -> Pose: SceneServiceException.__name__, upsert_collision.__name__, upsert_graspable.__name__, - graspable_state.__name__, - graspable_position.__name__, delete_collision_id.__name__, collision_ids.__name__, focus.__name__, diff --git a/src/python/arcor2_ur/common.py b/src/python/arcor2_ur/common.py index baa1e06b..1fc2df48 100644 --- a/src/python/arcor2_ur/common.py +++ b/src/python/arcor2_ur/common.py @@ -5,6 +5,7 @@ from flask import request from arcor2.data import common, object_type +from arcor2_ur.exceptions import UrGeneral @dataclass @@ -15,6 +16,9 @@ class CollisionSceneObject: def parse_collision_body() -> tuple[common.Pose, dict[str, Any]]: + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose or pose and metadata.") + body = humps.decamelize(request.json) if "pose" in body: diff --git a/src/python/arcor2_ur/data/moveit.yaml b/src/python/arcor2_ur/data/moveit.yaml index 08e944c4..a31eabf8 100644 --- a/src/python/arcor2_ur/data/moveit.yaml +++ b/src/python/arcor2_ur/data/moveit.yaml @@ -11,7 +11,7 @@ planning_pipelines: pipeline_names: ["ompl"] plan_request_params: - planning_attempts: 5 + planning_attempts: 3 planning_pipeline: ompl planner_id: RRTConnectkConfigDefault max_velocity_scaling_factor: 1.0 @@ -39,4 +39,13 @@ ompl: response_adapters: - default_planning_response_adapters/AddTimeOptimalParameterization - default_planning_response_adapters/ValidateSolution - - default_planning_response_adapters/DisplayMotionPath \ No newline at end of file + - default_planning_response_adapters/DisplayMotionPath + + planner_configs: + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 + + ur_manipulator: + planner_configs: + - RRTConnectkConfigDefault \ No newline at end of file diff --git a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl index 3c94a3d0..67b71ffa 100644 --- a/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl +++ b/src/python/arcor2_ur/data/urdf/meshes/ur5e/collision/suction_cup_1cup.stl @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2faa852002f4d06edc18faf5f9de3753e5e46d9349f75acea44812ac25b28a2c -size 249334 +oid sha256:10686a45842b55d7ce5dc6d3dbf1e51b235a1722e4697e4cec2ccf22e65bbfc4 +size 250034 diff --git a/src/python/arcor2_ur/data/urdf/ur5e.srdf b/src/python/arcor2_ur/data/urdf/ur5e.srdf index 3e6805bd..31df2964 100644 --- a/src/python/arcor2_ur/data/urdf/ur5e.srdf +++ b/src/python/arcor2_ur/data/urdf/ur5e.srdf @@ -5,16 +5,113 @@ - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/python/arcor2_ur/data/urdf/ur5e.urdf b/src/python/arcor2_ur/data/urdf/ur5e.urdf index 5699a9f0..f2754ef5 100644 --- a/src/python/arcor2_ur/data/urdf/ur5e.urdf +++ b/src/python/arcor2_ur/data/urdf/ur5e.urdf @@ -290,12 +290,17 @@ - - - - - - + + + + + + + + + + + @@ -304,17 +309,22 @@ - - - - - - + + + + + + + + + + + - + diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index 4b682b13..e95440d3 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -7,7 +7,7 @@ from arcor2.data import object_type from arcor2.data.common import ActionMetadata, Joint, Pose, Position, StrEnum from arcor2.data.robot import InverseKinematicsRequest -from arcor2_object_types.abstract import EffectorType, GraspPosition, Robot, Settings +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition, Robot, Settings from arcor2_web import rest @@ -285,6 +285,26 @@ def place_object( timeout=rest.Timeout(read=120), ) + def graspable_state(self, object_id: str) -> GraspableState: + """Returns graspable object state from the UR service.""" + + return GraspableState( + rest.call( + rest.Method.GET, + f"{self.settings.url}/graspable/{object_id}/state", + return_type=str, + ) + ) + + def graspable_position(self, object_id: str) -> Position: + """Returns graspable object position from the UR service.""" + + return rest.call( + rest.Method.GET, + f"{self.settings.url}/graspable/{object_id}/position", + return_type=Position, + ) + def suck( self, vacuum: int = 60, diff --git a/src/python/arcor2_ur/scripts/grasp_planner.py b/src/python/arcor2_ur/scripts/grasp_planner.py index 99a2a963..1ec9a260 100644 --- a/src/python/arcor2_ur/scripts/grasp_planner.py +++ b/src/python/arcor2_ur/scripts/grasp_planner.py @@ -9,8 +9,8 @@ from arcor2_object_types.abstract import EffectorType, GraspPosition from arcor2_ur.common import CollisionSceneObject -PRE_GRASP_OFFSET = 0.2 -GRASP_OFFSET = -0.01 +PRE_GRASP_OFFSET = 0.12 +GRASP_OFFSET = 0.06 def generate_grasp_poses( @@ -26,6 +26,8 @@ def generate_grasp_poses( candidates = create_grasp_candidates_from_surfaces(geometry, effector_type) + candidates.sort(key=candidate_sort_key) + return candidates[:20] @@ -101,12 +103,12 @@ def filter_geometry_by_grasp_position( normals = np.asarray(mesh.triangle_normals) wanted_dirs = { - GraspPosition.TOP: (0.0, 0.0, 1.0), GraspPosition.BOTTOM: (0.0, 0.0, -1.0), - GraspPosition.RIGHT: (1.0, 0.0, 0.0), - GraspPosition.LEFT: (-1.0, 0.0, 0.0), GraspPosition.BACK: (0.0, 1.0, 0.0), GraspPosition.FRONT: (0.0, -1.0, 0.0), + GraspPosition.LEFT: (-1.0, 0.0, 0.0), + GraspPosition.RIGHT: (1.0, 0.0, 0.0), + GraspPosition.TOP: (0.0, 0.0, 1.0), } kept = [] @@ -231,3 +233,20 @@ def normalize_orientation(q: Orientation) -> Orientation: def cross(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]: return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]) + + +def candidate_sort_key(candidate: tuple[Pose, Pose]) -> tuple[float, float, float, float]: + pre_grasp_pose, grasp_pose = candidate + + return ( + -grasp_pose.position.z, + abs(grasp_pose.position.y), + abs(grasp_pose.position.x), + distance_from_origin(pre_grasp_pose), + ) + + +def distance_from_origin(pose: Pose) -> float: + return math.sqrt( + pose.position.x * pose.position.x + pose.position.y * pose.position.y + pose.position.z * pose.position.z + ) diff --git a/src/python/arcor2_ur/scripts/robot_publisher.py b/src/python/arcor2_ur/scripts/robot_publisher.py index d02dde08..cd88e220 100644 --- a/src/python/arcor2_ur/scripts/robot_publisher.py +++ b/src/python/arcor2_ur/scripts/robot_publisher.py @@ -1,12 +1,12 @@ -import os -import subprocess as sp from importlib import resources import rclpy # pants: no-infer-dep +from geometry_msgs.msg import TransformStamped # pants: no-infer-dep from rclpy.executors import ExternalShutdownException # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep -from std_msgs.msg import Bool, String # pants: no-infer-dep +from std_msgs.msg import Bool # pants: no-infer-dep +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster # pants: no-infer-dep from ur_dashboard_msgs.msg import RobotMode # pants: no-infer-dep from arcor2_ur.topics import ROBOT_MODE_TOPIC, ROBOT_PROGRAM_RUNNING_TOPIC @@ -28,21 +28,13 @@ def load_robot_description() -> str: def load_robot_description_semantic() -> str: - ur_type = os.getenv("ARCOR2_UR_TYPE", "ur5e") - cmd = [ - "bash", - "-lc", - ( - "source /opt/ros/jazzy/setup.bash >/dev/null && " - f"xacro /opt/ros/jazzy/share/ur_moveit_config/srdf/ur.srdf.xacro name:={ur_type}" - ), - ] - return sp.check_output(cmd, text=True).strip() + srdf_res = resources.files("arcor2_ur").joinpath("data/urdf/ur5e.srdf") + return srdf_res.read_text(encoding="utf-8") class PublisherNode(Node): def __init__(self) -> None: - super().__init__("minimal_publisher") + super().__init__("arcor2_ur_test_robot_publisher") qos = QoSProfile( depth=1, @@ -52,22 +44,37 @@ def __init__(self) -> None: self.robot_program_running_pub = self.create_publisher(Bool, ROBOT_PROGRAM_RUNNING_TOPIC, qos) self.robot_mode_pub = self.create_publisher(RobotMode, ROBOT_MODE_TOPIC, qos) - self.robot_description_pub = self.create_publisher(String, "/robot_description", qos) - self.robot_description_semantic_pub = self.create_publisher(String, "/robot_description_semantic", qos) - - self.robot_description_msg = String() - self.robot_description_msg.data = load_robot_description() - - self.robot_description_semantic_msg = String() - self.robot_description_semantic_msg.data = load_robot_description_semantic() + self.static_tf_broadcaster = StaticTransformBroadcaster(self) + self._publish_static_transforms() self.timer = self.create_timer(1.0, self._republish) + def _publish_static_transforms(self) -> None: + transforms = [ + self._transform("tool0", "suction_base_link", 0.0, 0.0, 0.0), + self._transform("suction_base_link", "suction_cup_link", 0.0, 0.0, 0.1025), + self._transform("suction_cup_link", "suction_tcp", 0.0, 0.0, 0.0), + ] + + self.static_tf_broadcaster.sendTransform(transforms) + + def _transform(self, parent_frame: str, child_frame: str, x: float, y: float, z: float) -> TransformStamped: + transform = TransformStamped() + transform.header.stamp = self.get_clock().now().to_msg() + transform.header.frame_id = parent_frame + transform.child_frame_id = child_frame + transform.transform.translation.x = x + transform.transform.translation.y = y + transform.transform.translation.z = z + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = 0.0 + transform.transform.rotation.w = 1.0 + return transform + def _republish(self) -> None: self.robot_program_running_pub.publish(Bool(data=True)) self.robot_mode_pub.publish(RobotMode(mode=RobotMode.RUNNING)) - self.robot_description_pub.publish(self.robot_description_msg) - self.robot_description_semantic_pub.publish(self.robot_description_semantic_msg) def main() -> None: diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index 90c5c8e3..c180efa3 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -67,7 +67,20 @@ def pose_to_ros_pose(ps: Pose) -> RosPose: return rp -def mesh_file_to_ros_mesh(mesh_data: bytes, asset_id: str) -> RosMesh: +def mesh_scale_from_metadata(metadata: dict[str, Any]) -> tuple[float, float, float]: + scale = metadata.get("mesh_scale", (1.0, 1.0, 1.0)) + + if not isinstance(scale, (list, tuple)) or len(scale) != 3: + raise UrGeneral("Invalid mesh scale metadata.") + + return float(scale[0]), float(scale[1]), float(scale[2]) + + +def mesh_file_to_ros_mesh( + mesh_data: bytes, + asset_id: str, + scale: tuple[float, float, float] = (1.0, 1.0, 1.0), +) -> RosMesh: if not mesh_data: raise UrGeneral(f"Mesh asset '{asset_id}' is empty.") @@ -84,7 +97,14 @@ def mesh_file_to_ros_mesh(mesh_data: bytes, asset_id: str) -> RosMesh: if loaded.is_empty(): raise UrGeneral(f"Mesh asset '{asset_id}' has no geometry.") - vertices = tuple((float(vertex[0]), float(vertex[1]), float(vertex[2])) for vertex in loaded.vertices) + vertices = tuple( + ( + float(vertex[0]) * scale[0], + float(vertex[1]) * scale[1], + float(vertex[2]) * scale[2], + ) + for vertex in loaded.vertices + ) triangles = tuple((int(triangle[0]), int(triangle[1]), int(triangle[2])) for triangle in loaded.triangles) if not vertices or not triangles: @@ -443,7 +463,7 @@ def __init__( self.base_link = base_link self.tool_link = tool_link self.collision_objects = collision_objects.copy() - self.mesh_models: dict[str, RosMesh] = {} + self.mesh_models: dict[tuple[str, tuple[float, float, float]], RosMesh] = {} self.joints: list[Joint] = [] self.tool: VGC10 | None = None self.freedrive_mode = False @@ -762,37 +782,29 @@ def apply_collision_objects(self, scene) -> None: pose_in_frame = Pose.from_dict(cast(dict[str, Any], attached_pose_dict)) - attached = self.create_collision_object(obj, obj_id, self.tool_link, pose_in_frame, self.tool_link) + attached = self.create_collision_object(obj, pose_in_frame, attach=True) scene.process_attached_collision_object(attached) logger.info(f"Attached object id: {obj_id}") continue else: # normal collision object pose_in_frame = tr.make_pose_rel(self.base_pose, obj.pose) - collision = self.create_collision_object(obj, obj_id, self.base_link, pose_in_frame) + collision = self.create_collision_object(obj, pose_in_frame) scene.apply_collision_object(collision) - if state == GraspableState.TOUCH_ALLOWED: - self.allow_collision_between_object_and_links( - scene, obj_id, [self.tool_link, "suction_base_link", "suction_cup_link", "suction_tcp"] - ) # TODO auto - scene.current_state.update(force=True) scene.current_state.update() - # remove id ? def create_collision_object( - self, - obj: CollisionSceneObject, - obj_id: str, - frame_id: str, - pose_in_frame: Pose, - attached_to_link: str | None = None, + self, obj: CollisionSceneObject, pose_in_frame: Pose, attach: bool = False ) -> CollisionObject | AttachedCollisionObject: collision_object = CollisionObject() - collision_object.header.frame_id = frame_id - collision_object.id = obj_id + if attach: + collision_object.header.frame_id = self.tool_link + else: + collision_object.header.frame_id = self.base_link + collision_object.id = obj.model.id collision_object.operation = CollisionObject.ADD if isinstance(obj.model, object_type.Box): @@ -821,67 +833,27 @@ def create_collision_object( elif isinstance(obj.model, object_type.Mesh): asset_id = obj.model.asset_id - cached_mesh = self.mesh_models.get(asset_id) + scale = mesh_scale_from_metadata(obj.metadata) + cache_key = (asset_id, scale) + cached_mesh = self.mesh_models.get(cache_key) if cached_mesh is None: mesh_data = storage_client.get_asset_data(asset_id) - cached_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id) - self.mesh_models[asset_id] = cached_mesh + cached_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id, scale) + self.mesh_models[cache_key] = cached_mesh collision_object.meshes.append(copy.deepcopy(cached_mesh)) collision_object.mesh_poses.append(pose_to_ros_pose(pose_in_frame)) - if attached_to_link is not None: + if attach: attached = AttachedCollisionObject() - attached.link_name = attached_to_link + attached.link_name = self.tool_link attached.object = collision_object - attached.touch_links = [attached_to_link, "suction_base_link", "suction_cup_link"] + attached.touch_links = [self.tool_link, "suction_base_link", "suction_cup_link"] # robot parts return attached return collision_object - def allow_collision_between_object_and_links(self, scene, object_id: str, link_names: list[str]) -> None: - acm = scene.planning_scene_message.allowed_collision_matrix - - names = list(acm.entry_names) - values = list(acm.entry_values) - - for name in [object_id, *link_names]: - if name not in names: - names.append(name) - for row in values: - row.enabled.append(False) - - new_row = copy.deepcopy(values[0]) if values else type(acm.entry_values[0])() - new_row.enabled = [False] * len(names) - values.append(new_row) - - for link_name in link_names: - i = names.index(object_id) - j = names.index(link_name) - values[i].enabled[j] = True - values[j].enabled[i] = True - - acm.entry_names = names - acm.entry_values = values - - def filter_grasps_by_ik(self, grasp_options: list[tuple[Pose, Pose]]) -> list[tuple[Pose, Pose]]: - valid = [] - - for pre_grasp_pose, grasp_pose in grasp_options: - try: - self.inverse_kinematics( - InverseKinematicsRequest(pose=pre_grasp_pose, start_joints=self.joints, avoid_collisions=True) - ) - self.inverse_kinematics( - InverseKinematicsRequest(pose=grasp_pose, start_joints=self.joints, avoid_collisions=False) - ) - valid.append((pre_grasp_pose, grasp_pose)) - except Exception: - continue - - return valid - def get_joints(self) -> list[dict]: return [joint.to_dict() for joint in self.joints] @@ -983,20 +955,37 @@ def attach_object( ros_mesh = None if isinstance(object.model, object_type.Mesh): asset_id = object.model.asset_id - ros_mesh = self.mesh_models.get(asset_id) + scale = mesh_scale_from_metadata(object.metadata) + cache_key = (asset_id, scale) + ros_mesh = self.mesh_models.get(cache_key) if ros_mesh is None: mesh_data = storage_client.get_asset_data(asset_id) - ros_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id) - self.mesh_models[asset_id] = ros_mesh + ros_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id, scale) + self.mesh_models[cache_key] = ros_mesh grasp_options = generate_grasp_poses(object, effector_type, grasp_positions, ros_mesh) logger.info(f"Generated {len(grasp_options)} grasp options for object {object_id}.") - grasp_options = self.filter_grasps_by_ik(grasp_options) - logger.info(f"IK accepted: {len(grasp_options)} grasp options.") - + trail = 0 for pre_grasp_pose, grasp_pose in grasp_options: + trail += 1 + try: + self.inverse_kinematics( + InverseKinematicsRequest(pose=pre_grasp_pose, start_joints=self.joints, avoid_collisions=False) + ) + self.inverse_kinematics( + InverseKinematicsRequest(pose=grasp_pose, start_joints=self.joints, avoid_collisions=False) + ) + logger.info(f"IK accepted: {trail} grasp options.") + except Exception as exc: + logger.exception( + f"IK rejected: {trail}/{len(grasp_options)} grasp candidate. " + f"pre_grasp_pose={pre_grasp_pose.to_dict()}, " + f"grasp_pose={grasp_pose.to_dict()}: {exc}" + ) + continue + try: self.move_to_pose(pre_grasp_pose, velocity, payload, safe) @@ -1004,7 +993,7 @@ def attach_object( self.suck(60) time.sleep(1.0) - object.metadata["state"] = GraspableState.TOUCH_ALLOWED + object.metadata["state"] = GraspableState.HIDDEN self.update_collisions(self.collision_objects) self.move_to_pose(grasp_pose, velocity, payload, safe) @@ -1021,7 +1010,21 @@ def attach_object( self.update_collisions(self.collision_objects) - except Exception: + except Exception as exc: + logger.exception( + f"Failed grasp candidate {trail}/{len(grasp_options)} " + f"for object {object_id}. " + f"pre_grasp_pose={pre_grasp_pose.to_dict()}, " + f"grasp_pose={grasp_pose.to_dict()}: {exc}" + ) + + if self.tool: + self.release() + + object.metadata.pop("attached_pose", None) + object.metadata["state"] = GraspableState.WORLD + self.update_collisions(self.collision_objects) + continue return object.metadata.copy() @@ -1056,8 +1059,6 @@ def detach_object( self.release() time.sleep(1.0) - object.metadata["state"] = GraspableState.TOUCH_ALLOWED - self.remove_attached_object(object_id) object.metadata.pop("attached_pose", None) object.pose = target_pose self.update_collisions(self.collision_objects) diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index c7daa11c..b8f37f6b 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -228,13 +228,23 @@ def get_started() -> RespT: @app.route("/graspable//state", methods=["GET"]) def get_graspable_state(object_id: str) -> RespT: """Return graspable object state.""" - return jsonify(globs.collision_objects[object_id].metadata["state"]) + try: + obj = globs.collision_objects[object_id] + except KeyError: + raise NotFound("Graspable object not found") + + return jsonify(obj.metadata["state"]) @app.route("/graspable//position", methods=["GET"]) def get_graspable_position(object_id: str) -> RespT: """Return graspable object position.""" - return jsonify(globs.collision_objects[object_id].pose.position.to_dict()) + try: + obj = globs.collision_objects[object_id] + except KeyError: + raise NotFound("Graspable object not found") + + return jsonify(obj.pose.position.to_dict()) @app.route("/graspable/pick_up_object_by_position", methods=["PUT"]) @@ -449,7 +459,17 @@ def pick_up_object_by_id() -> RespT: payload = float(body.get("payload", 0.0)) safe = bool(body.get("safe", True)) - selected = globs.collision_objects[object_id] + try: + selected = globs.collision_objects[object_id] + except KeyError: + raise NotFound("Graspable object not found") + + if selected.metadata.get("object_type") != "graspable": + raise UrGeneral(f"Object {object_id} is not graspable.") + + if selected.metadata.get("state") != GraspableState.WORLD: + raise UrGeneral(f"Object {object_id} is not in WORLD state.") + previous_state = selected.metadata.get("state") selected.metadata["state"] = GraspableState.RESERVED @@ -758,6 +778,16 @@ def put_mesh() -> RespT: pose, metadata = parse_collision_body() args = humps.decamelize(request.args.to_dict()) + + mesh_scale = ( + float(args.get("mesh_scale_x", 1.0)), + float(args.get("mesh_scale_y", 1.0)), + float(args.get("mesh_scale_z", 1.0)), + ) + + metadata = metadata.copy() + metadata["mesh_scale"] = mesh_scale + mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) globs.collision_objects[mesh.id] = CollisionSceneObject(mesh, pose, metadata) diff --git a/src/python/arcor2_ur/tests/BUILD b/src/python/arcor2_ur/tests/BUILD index 7e07d76e..4541c50d 100644 --- a/src/python/arcor2_ur/tests/BUILD +++ b/src/python/arcor2_ur/tests/BUILD @@ -17,6 +17,7 @@ python_tests( "test_collision/test_*.py", "test_pick_up/test_*.py", "test_pick_and_place/test_*.py", + "test_pick_and_move_whit_collisions/test_*.py", ], dependencies=[ ":test_utils", diff --git a/src/python/arcor2_ur/tests/conftest.py b/src/python/arcor2_ur/tests/conftest.py index 4f50caf7..93158619 100644 --- a/src/python/arcor2_ur/tests/conftest.py +++ b/src/python/arcor2_ur/tests/conftest.py @@ -4,8 +4,7 @@ import signal import subprocess as sp import time -from pathlib import Path -from typing import Iterator, NamedTuple +from typing import Any, Iterator, NamedTuple import pytest @@ -22,7 +21,7 @@ class Urls(NamedTuple): storage_url: str -def _finish_processes(processes) -> None: +def _finish_processes(processes: list[sp.Popen]) -> None: for proc in processes: if proc.poll() is None: try: @@ -43,14 +42,13 @@ def _finish_processes(processes) -> None: def _load_ros_env() -> dict[str, str]: - """Load environment variables from ROS setup script.""" try: output = sp.check_output( ["bash", "-lc", "source /opt/ros/jazzy/setup.bash >/dev/null && env -0"], text=False, env={}, ) - except sp.CalledProcessError as exc: # pragma: no cover - best effort helper + except sp.CalledProcessError as exc: raise RuntimeError("Failed to source /opt/ros/jazzy/setup.bash") from exc env: dict[str, str] = {} @@ -65,74 +63,53 @@ def _load_ros_env() -> dict[str, str]: return env +def _ros_launch_env(env: dict[str, str]) -> dict[str, str]: + launch_env = {key: value for key, value in env.items() if not key.startswith("PEX_")} + + for key in ("PYTEST_DISABLE_PLUGIN_AUTOLOAD", "PYTEST_PLUGINS", "PYTHONDEVMODE", "PYTHONWARNINGS"): + launch_env.pop(key, None) + + return launch_env + + +def _assert_process_alive(proc: sp.Popen, error_message: str) -> None: + if proc.poll() is not None: + log_proc_output(proc.communicate()) + raise RuntimeError(error_message) + + @pytest.fixture(scope="module", params=["ur5e"]) def start_processes(request) -> Iterator[Urls]: - """Starts UR test dependencies.""" - ros_domain_id = str(random.sample(range(0, 232 + 1), 1)[0]) ur_type: str = request.param - processes = [] + processes: list[sp.Popen] = [] ros_env = _load_ros_env() + my_env = os.environ.copy() my_env.update(ros_env) my_env["ROS_DOMAIN_ID"] = ros_domain_id - # Keep ROS view of the world dominant; strip test-only/debug env for the ROS launcher. - ros_launch_env = {k: v for k, v in my_env.items() if not k.startswith("PEX_")} - - for key in ("PYTEST_DISABLE_PLUGIN_AUTOLOAD", "PYTEST_PLUGINS", "PYTHONDEVMODE", "PYTHONWARNINGS"): - ros_launch_env.pop(key, None) + ros_launch_env = _ros_launch_env(my_env) - kwargs = { + kwargs: dict[str, Any] = { "env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT, "start_new_session": True, } - # Start ROS control with mock hardware. - processes.append( - sp.Popen( - [ - "ros2", - "launch", - "ur_robot_driver", - "ur_control.launch.py", - "launch_rviz:=false", - f"ur_type:={ur_type}", - "use_mock_hardware:=true", - "robot_ip:=xyz", - ], - env=ros_launch_env, - stdout=sp.PIPE, - stderr=sp.STDOUT, - start_new_session=True, - ) - ) - - time.sleep(3) - - if processes[-1].poll(): - log_proc_output(processes[-1].communicate()) - raise Exception("Launch died.") - - # Kill default robot_state_publisher. - sp.run("pkill -f robot_state_publisher", shell=True) - - # Start custom robot_state_publisher with modified URDF. - urdf_path = Path(__file__).resolve().parents[1] / "data" / "urdf" / "ur5e.urdf" - - custom_rsp = sp.Popen( + ur_control_proc = sp.Popen( [ "ros2", - "run", - "robot_state_publisher", - "robot_state_publisher", - "--ros-args", - "-p", - f"robot_description:={urdf_path.read_text()}", + "launch", + "ur_robot_driver", + "ur_control.launch.py", + "launch_rviz:=false", + f"ur_type:={ur_type}", + "use_mock_hardware:=true", + "robot_ip:=xyz", ], env=ros_launch_env, stdout=sp.PIPE, @@ -140,11 +117,11 @@ def start_processes(request) -> Iterator[Urls]: start_new_session=True, ) - processes.append(custom_rsp) + processes.append(ur_control_proc) - time.sleep(2) + time.sleep(3) + _assert_process_alive(ur_control_proc, "UR control launch died.") - # Start ARCOR2 storage service. storage_port = find_free_port() storage_url = f"http://127.0.0.1:{storage_port}" @@ -157,13 +134,13 @@ def start_processes(request) -> Iterator[Urls]: storage_proc = sp.Popen( ["python", "src.python.arcor2_storage.scripts/storage.pex"], **kwargs, - ) # type: ignore + ) processes.append(storage_proc) - if storage_proc.poll(): + if storage_proc.poll() is not None: _finish_processes(processes) - raise Exception("Storage service died.") + raise RuntimeError("Storage service died.") try: check_health("Storage", storage_url, timeout=20) @@ -171,7 +148,6 @@ def start_processes(request) -> Iterator[Urls]: _finish_processes(processes) raise - # Start UR service. robot_url = f"http://0.0.0.0:{find_free_port()}" my_env["ARCOR2_UR_URL"] = robot_url @@ -183,13 +159,13 @@ def start_processes(request) -> Iterator[Urls]: robot_proc = sp.Popen( ["python", "src.python.arcor2_ur.scripts/ur.pex"], **kwargs, - ) # type: ignore + ) processes.append(robot_proc) - if robot_proc.poll(): + if robot_proc.poll() is not None: _finish_processes(processes) - raise Exception("Robot service died.") + raise RuntimeError("Robot service died.") try: check_health("UR", robot_url, timeout=20) @@ -197,17 +173,16 @@ def start_processes(request) -> Iterator[Urls]: _finish_processes(processes) raise - # robot_mode etc. is not published with mock_hw -> helper node publishes it. robot_pub_proc = sp.Popen( ["python", "src.python.arcor2_ur.scripts/robot_publisher.pex"], **kwargs, - ) # type: ignore + ) processes.append(robot_pub_proc) - if robot_pub_proc.poll(): + if robot_pub_proc.poll() is not None: _finish_processes(processes) - raise Exception("Robot publisher node died.") + raise RuntimeError("Robot publisher node died.") yield Urls(ros_domain_id, robot_url, storage_url) diff --git a/src/python/arcor2_ur/tests/results_velka spatna.txt b/src/python/arcor2_ur/tests/results_velka spatna.txt deleted file mode 100644 index 7d59d879..00000000 --- a/src/python/arcor2_ur/tests/results_velka spatna.txt +++ /dev/null @@ -1,28886 +0,0 @@ -12:01:03.71 [WARN] Pants cannot infer owners for the following imports in the target src/python/arcor2_ur/scripts/ros_worker.py: - - * moveit_msgs.msg.AttachedCollisionObject (line: 17) - * moveit_msgs.msg.CollisionObject (line: 18) - * moveit_msgs.msg.Constraints (line: 19) - * moveit_msgs.msg.OrientationConstraint (line: 20) - * moveit_msgs.msg.PositionConstraint (line: 21) - -If you do not expect an import to be inferable, add `# pants: no-infer-dep` to the import line. Otherwise, see https://www.pantsbuild.org/2.31/docs/using-pants/troubleshooting-common-issues#import-errors-and-missing-dependencies for common problems. -12:01:06.08 [INFO] Completed: Scheduling: Find interpreter for constraints: CPython==3.12.* -12:01:06.09 [INFO] Completed: Scheduling: Building local_dists.pex -12:01:06.11 [INFO] Completed: Scheduling: Building 21 requirements for src.python.arcor2_storage.scripts/storage.pex from the 3rdparty/constraints.txt resolve: Flask~=3.1.2, Pillow~=12.1.1, aiologger~=0.7.0, apispec-webframeworks~... (467 characters truncated) -12:01:06.11 [INFO] Completed: Scheduling: Building 3 requirements for src.python.arcor2_ur.scripts/robot_publisher.pex from the 3rdparty/constraints.txt resolve: PyYAML~=6.0.3, numpy~=2.3.5, open3d==0.19.0 -12:01:06.11 [INFO] Completed: Scheduling: Building 26 requirements for src.python.arcor2_ur.scripts/ur.pex from the 3rdparty/constraints.txt resolve: Flask~=3.1.2, Pillow~=12.1.1, PyYAML~=6.0.3, aiologger~=0.7.0, apispec-webframew... (535 characters truncated) -12:01:06.39 [INFO] Completed: Scheduling: Building 17 requirements for requirements.pex from the 3rdparty/constraints.txt resolve: Pillow~=12.1.1, aiologger~=0.7.0, colorlog~=6.10.1, dataclasses-jsonschema[apispec,fast-dateparsing... (329 characters truncated) -12:01:06.70 [INFO] Completed: Scheduling: Building pytest.pex -12:01:06.71 [INFO] Completed: Scheduling: Building 20 requirements for requirements.pex from the 3rdparty/constraints.txt resolve: Pillow~=12.1.1, PyYAML~=6.0.3, aiologger~=0.7.0, colorlog~=6.10.1, dataclasses-jsonschema[apispec,f... (406 characters truncated) -12:01:06.72 [INFO] Completed: Scheduling: Building 19 requirements for requirements.pex from the 3rdparty/constraints.txt resolve: Pillow~=12.1.1, PyYAML~=6.0.3, aiologger~=0.7.0, colorlog~=6.10.1, dataclasses-jsonschema[apispec,f... (360 characters truncated) -12:01:06.83 [INFO] Completed: Scheduling: Test binary /bin/bash. -12:01:06.83 [INFO] Completed: Scheduling: Test binary /usr/bin/bash. -12:01:06.85 [INFO] Completed: Scheduling: Building pytest_runner.pex -12:01:06.85 [INFO] Completed: Scheduling: Building pytest_runner.pex -12:01:06.85 [INFO] Completed: Scheduling: Building pytest_runner.pex -12:01:06.89 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_interaction.py -12:01:06.91 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_interaction.py - succeeded. -12:01:18.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py -12:01:48.37 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests -12:01:53.37 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests -12:02:02.95 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests -12:02:27.81 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests -12:02:32.25 [INFO] Long running tasks: - 85.38s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:02:39.49 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests -12:03:02.32 [INFO] Long running tasks: - 73.95s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests - 115.45s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:03:25.42 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests -12:03:32.36 [INFO] Long running tasks: - 64.56s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 103.99s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests - 145.49s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:03:57.17 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:04:02.42 [INFO] Long running tasks: - 94.61s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 175.55s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:04:23.86 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests -12:04:32.46 [INFO] Long running tasks: - 124.66s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 205.59s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:04:39.75 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:04:43.98 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests -12:05:02.51 [INFO] Long running tasks: - 154.70s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:05:12.94 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests -12:05:32.55 [INFO] Long running tasks: - 68.69s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - 184.74s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:05:46.51 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests -12:05:51.64 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests -12:05:57.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:06:02.61 [INFO] Long running tasks: - 98.76s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests -12:06:23.33 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests -12:06:32.65 [INFO] Long running tasks: - 128.79s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests -12:06:32.76 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests -12:07:02.68 [INFO] Long running tasks: - 65.32s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 76.17s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:07:07.26 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py:../tests -12:07:07.27 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py:../tests - succeeded. -12:07:09.66 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests -12:07:32.74 [INFO] Long running tasks: - 95.38s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 106.23s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:07:53.35 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests -12:08:02.78 [INFO] Long running tasks: - 125.42s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 136.27s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:08:30.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests -12:08:32.81 [INFO] Long running tasks: - 155.46s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 166.31s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:08:45.68 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests -12:08:49.17 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_ur.py -12:08:49.17 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_ur.py - succeeded. -12:09:02.86 [INFO] Long running tasks: - 185.50s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 196.35s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:09:02.96 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests -12:09:05.06 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:09:15.78 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py -12:09:19.20 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:09:21.58 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests -12:09:35.71 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests -12:09:49.56 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests -12:10:03.41 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests -12:10:08.45 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests -12:10:14.66 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests -12:10:33.20 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests -12:10:48.75 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests -12:11:03.91 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests -12:11:33.10 [INFO] Long running tasks: - 84.65s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:11:37.41 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests -12:11:46.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests -12:12:03.16 [INFO] Long running tasks: - 89.96s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 114.72s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:12:11.16 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests -12:12:17.75 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:12:33.22 [INFO] Long running tasks: - 120.02s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:12:50.45 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests -12:12:56.56 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests -12:13:03.26 [INFO] Long running tasks: - 150.06s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:13:33.30 [INFO] Long running tasks: - 82.14s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 180.09s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:13:51.60 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:13:54.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests -12:14:03.33 [INFO] Long running tasks: - 72.89s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - 112.18s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:14:33.38 [INFO] Long running tasks: - 102.93s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - 142.22s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:14:34.48 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests -12:14:35.49 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests -12:14:41.62 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests -12:15:03.44 [INFO] Long running tasks: - 172.29s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:15:09.77 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests -12:15:20.55 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py -12:15:33.49 [INFO] Long running tasks: - 202.33s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:15:45.14 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:15:57.09 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests -12:16:03.53 [INFO] Long running tasks: - 81.91s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:16:18.18 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests -12:16:31.93 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests -12:16:33.58 [INFO] Long running tasks: - 73.03s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 111.96s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:16:52.27 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests -12:17:03.63 [INFO] Long running tasks: - 103.08s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 142.00s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:17:26.65 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests -12:17:33.65 [INFO] Long running tasks: - 61.72s Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests - 133.10s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 172.03s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:17:34.01 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests -12:18:03.69 [INFO] Long running tasks: - 163.15s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 202.07s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:18:09.73 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests -12:18:12.87 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:18:33.74 [INFO] Long running tasks: - 193.19s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:18:35.78 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:18:39.06 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests -12:19:06.22 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests -12:19:09.29 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests -12:19:14.24 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests -12:19:14.24 [INFO] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests - succeeded. -12:19:31.72 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests -12:19:38.74 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests -12:19:48.01 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests -12:20:12.90 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests -12:20:33.94 [INFO] Long running tasks: - 84.65s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:20:45.67 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests -12:21:01.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests -12:21:03.98 [INFO] Long running tasks: - 85.24s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 114.69s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:21:23.51 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:21:34.03 [INFO] Long running tasks: - 115.29s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:21:42.12 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests -12:21:51.81 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py -12:21:56.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests -12:21:58.94 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests -12:22:04.08 [INFO] Long running tasks: - 145.34s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:22:10.12 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:22:40.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests -12:22:44.90 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests -12:23:04.17 [INFO] Long running tasks: - 67.80s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests - 72.36s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:23:12.47 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests -12:23:17.53 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests -12:23:21.02 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests -12:23:34.25 [INFO] Long running tasks: - 102.44s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:23:57.42 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests -12:24:03.53 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests -12:24:04.31 [INFO] Long running tasks: - 132.50s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:24:34.37 [INFO] Long running tasks: - 73.35s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 162.56s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:24:49.65 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests -12:25:04.41 [INFO] Long running tasks: - 66.99s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 103.39s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 192.60s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:25:25.49 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:25:34.45 [INFO] Long running tasks: - 97.03s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 133.43s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:25:37.62 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests -12:25:59.88 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests -12:26:04.51 [INFO] Long running tasks: - 127.09s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 163.49s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:26:25.37 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests -12:26:33.52 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:26:34.55 [INFO] Long running tasks: - 157.13s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:26:36.55 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests -12:26:59.16 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests -12:27:04.61 [INFO] Long running tasks: - 187.19s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:27:32.96 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:27:34.67 [INFO] Long running tasks: - 61.14s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests -12:27:48.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests -12:27:55.34 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests -12:28:04.70 [INFO] Long running tasks: - 91.18s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests -12:28:06.22 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_suck_effector.py -12:28:06.23 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_suck_effector.py - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-Tx0YCO -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_suck_effector.py::test_suck_effector[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:28:05 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-27-56-910827-DESKTOP-HG3Q5KV-27852 (testutils.py:33) - -2026-05-10 12:28:05 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [27859] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [27860] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [27861] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-4]: process started with pid [27862] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-5]: process started with pid [27863] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.262355366] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.265306507] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778408877.265547580] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.268420450] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.268437803] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.268441811] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408877.268543033] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.437860768] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.440923939] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.441738537] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.441850419] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.450163280] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451553482] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451584381] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451636148] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451649484] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451739815] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.451811982] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.565168373] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.817227969] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.817293463] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.820292879] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.844506788] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.845677854] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.845936956] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.846002861] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.846033409] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.850651427] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.857973365] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.858015245] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.859064499] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.872216249] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.872494898] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.873314225] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.876105895] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.876142073] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.876883507] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.889846970] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.890133063] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.890659924] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.892238685] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.892256408] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.892827714] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.904033225] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.904279914] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.904715772] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.906323036] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.906359135] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.910187540] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.923775162] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.924053687] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.932449359] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.932485348] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.932742026] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.945770801] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.946032684] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.952046932] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.952086958] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.952462411] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.966332532] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.966625964] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.970618645] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.970656067] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.971128484] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-5] [INFO] [1778408877.983880325] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408877.984156811] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.067755179] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 27863] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.320264589] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.320330243] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.321752576] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.336358699] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.337372075] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.337451245] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.342376314] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.343061296] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.346025742] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.346938773] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.346973479] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.347325428] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.362036285] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.362372483] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.363680574] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408878.374293431] [controller_manager]: Overrun might occur, Total time : 9448.083 us (Expected < 2000.000 us) --> Read time : 12.313 us, Update time : 9432.433 us (Switch time : 9384.542 us (Switch chained mode time : 0.662 us, perform mode change time : 10.710 us, Activation time : 9358.082 us, Deactivation time : 0.160 us)), Write time : 3.337 us (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408878.374356510] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.586517 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.374307067] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.376164455] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.377239573] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.377271183] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.377704972] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.390096869] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.405246826] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.406319725] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.406390429] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.410969053] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.412893871] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.417552378] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.419227010] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.419244694] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.421780647] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.450187558] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.453977926] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.462150650] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.464234079] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.465000675] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.468158520] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.470198538] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.470217594] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.470656363] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.488204049] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.488568136] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.491711058] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.492998349] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.495978237] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.497369735] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.497405874] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.497744688] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.510989105] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.528010061] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528574694] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528749977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528765727] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.528775596] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.531864716] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.554122152] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408878.555398052] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [spawner-4] [INFO] [1778408878.570292217] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 27862] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778408879.676500094] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 27860] (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [INFO] [1778408879.993677001] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408879.993751903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408881.922245432] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.475839 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [ros2_control_node-1] [WARN] [1778408883.126215134] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.445481 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [1778408879.872023528] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] [INFO] [1778408885.801445590] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] 2026-05-10 12:28:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Running on http://127.0.0.1:54107 (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] * Running on http://172.30.43.138:54107 (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:28:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:28:05] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) - - -=================================== FAILURES =================================== -___________________________ test_suck_effector[ur5e] ___________________________ - -start_processes = Urls(ros_domain_id='85', robot_url='http://0.0.0.0:54107', storage_url='http://127.0.0.1:57693') - - @pytest.mark.timeout(60) - def test_suck_effector(start_processes: Urls) -> None: -  - robot_description = load_robot_description() - assert "suction_base_link" in robot_description - assert "suction_cup_link" in robot_description - assert "suction_tcp" in robot_description -> assert "tool0_to_suction_base" in robot_description -E assert 'tool0_to_suction_base' in '\n\n\n\n\n\n Read time : 12.313 us, Update time : 9432.433 us (Switch time : 9384.542 us (Switch chained mode time : 0.662 us, perform mode change time : 10.710 us, Activation time : 9358.082 us, Deactivation time : 0.160 us)), Write time : 3.337 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408878.374356510] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.586517 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.374307067] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.376164455] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.377239573] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.377271183] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.377704972] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.390096869] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.405246826] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.406319725] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.406390429] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.410969053] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.412893871] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.417552378] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.419227010] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.419244694] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.421780647] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.450187558] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.453977926] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.462150650] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.464234079] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.465000675] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.468158520] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.470198538] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.470217594] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.470656363] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.488204049] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.488568136] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.491711058] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.492998349] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.495978237] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.497369735] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.497405874] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.497744688] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_wxnmhyfv -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.510989105] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.528010061] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528574694] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528749977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528765727] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.528775596] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.531864716] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.554122152] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408878.555398052] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408878.570292217] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 27862] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408879.676500094] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 27860] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408879.993677001] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408879.993751903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408881.922245432] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.475839 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408883.126215134] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.445481 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408879.872023528] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408885.801445590] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:28:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:54107 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:54107 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:28:05] "GET /healthz/ready HTTP/1.1" 200 - -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_suck_effector.py:10 - src/python/arcor2_ur/tests/test_suck_effector.py:10: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(60) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_suck_effector.py.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_suck_effector.py::test_suck_effector[ur5e] - assert 'tool0_to_suction_base' in '\n Read time : 14.507 us, Update time : 9700.844 us (Switch time : 9654.205 us (Switch chained mode time : 0.381 us, perform mode change time : 12.224 us, Activation time : 9624.458 us, Deactivation time : 0.180 us)), Write time : 2.254 us (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408966.620408695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.880177 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.620377296] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.621643902] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.622858525] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.622899924] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.623466775] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.642939124] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.645841138] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.646239791] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.646337587] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.649637192] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.650771587] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.653654304] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.655265967] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.655304480] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.656640855] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.677946071] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.678323414] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.680797175] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.683626491] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.684871446] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.687779657] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.689074823] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.689114148] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.689516392] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.707979148] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.708350514] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.711451221] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.712762358] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.715883604] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.717353260] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.717399999] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.717955419] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.735359690] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.752307543] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.752981192] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.753122721] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.753137639] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.753147398] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.754931753] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.764345126] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408966.764965224] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [spawner-4] [INFO] [1778408966.768961181] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [robot_state_publisher-2] [INFO] [1778408966.831056325] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 30307] (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 30305] (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408967.952895816] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408967.952950620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408969.067815810] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.287954 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408970.242220186] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.692100 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408973.093370250] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.842163 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408974.097789566] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.261560 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408975.182221851] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.693895 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408975.493127231] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408975.493178468] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408975.761371324] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408975.761406781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.525708663] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.180627 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.759130170] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.759166860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.759109321] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [ros2_control_node-1] [WARN] [1778408977.759157713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [INFO] [1778408967.838581671] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] [INFO] [1778408973.421448284] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:29:38 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] * Running on http://127.0.0.1:43109 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] * Running on http://172.30.43.138:43109 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:33] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.759627551] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.759664791] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.759674400] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.767103696] [moveit_1321688465.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.769480512] [moveit_1321688465.moveit.ros.rdf_loader]: Loaded robot model in 0.00218947 seconds (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.769521229] [moveit_1321688465.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.769534505] [moveit_1321688465.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [WARN] [1778408976.782308422] [moveit_1321688465.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.784827117] [moveit_1321688465.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.842885861] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.843029724] [moveit_1321688465.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844159581] [moveit_1321688465.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844705848] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844721188] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.844836857] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845239081] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845329123] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845741656] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.845753619] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.846418929] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408976.846909572] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [WARN] [1778408976.847130171] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [ERROR] [1778408976.847146892] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.026190048] [moveit_1321688465.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.027012000] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.028560699] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.028580166] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029042294] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029060158] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029087530] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029092890] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.029104763] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.030251060] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.032489603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.032505664] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.034504872] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.034520872] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.035246021] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.064593754] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.068137964] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.068329783] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.068354781] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408977.069257156] [moveit_1321688465.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:37] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:38] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408978.253042507] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:53 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:53 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] 2026-05-10 12:29:53 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:29:57 [ ERROR] [INFO] [1778408993.396324103] [moveit_1321688465.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -______________________ test_pick_up_box_by_position[ur5e] ______________________ - -start_processes = Urls(ros_domain_id='55', robot_url='http://0.0.0.0:43109', storage_url='http://127.0.0.1:40591') - - @pytest.mark.timeout(321) - def test_pick_up_box_by_position(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Box("Box1", 0.2, 0.2, 0.2) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Box1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-24-826127-DESKTOP-HG3Q5KV-30285 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [30304] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [30305] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [30306] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [30307] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [30308] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408965.114460682] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.118941956] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.121471017] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.124139793] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.124167926] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.124173507] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408965.124265422] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.291228511] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.293916068] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.294385070] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.294478658] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.302275922] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303637694] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303673372] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303698179] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303704060] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303766679] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.303807537] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.555397724] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.807211325] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.807264436] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.809804657] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.831778494] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.832828425] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.832966127] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.833002666] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.833025549] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.837011313] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.843725250] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.843767550] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.844659315] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.855331924] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.855559702] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.856318263] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.857725581] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.857759746] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.858483271] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.869374897] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.869601437] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.870226406] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.871847991] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.871883598] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.872555701] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.883458458] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.883674317] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.884210616] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.885827504] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.885861669] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.889386251] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.901494819] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.901737665] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.910012879] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.910050861] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.910369797] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.923699049] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.923939596] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.929747397] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.929778646] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.930051073] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.943489835] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.943706270] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.945973829] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.946027751] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.946604437] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408965.959742242] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408965.960028134] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.056723572] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 30308] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.564776222] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.564842337] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.566640233] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.583650610] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.584930797] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.585002874] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.590293423] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.590828789] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.593816360] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.594727577] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.594762874] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.595055420] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.607495067] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.607790885] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.609344350] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408966.620328632] [controller_manager]: Overrun might occur, Total time : 9717.605 us (Expected < 2000.000 us) --> Read time : 14.507 us, Update time : 9700.844 us (Switch time : 9654.205 us (Switch chained mode time : 0.381 us, perform mode change time : 12.224 us, Activation time : 9624.458 us, Deactivation time : 0.180 us)), Write time : 2.254 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408966.620408695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.880177 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.620377296] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.621643902] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.622858525] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.622899924] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.623466775] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.642939124] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.645841138] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.646239791] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.646337587] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.649637192] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.650771587] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.653654304] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.655265967] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.655304480] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.656640855] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.677946071] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.678323414] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.680797175] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.683626491] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.684871446] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.687779657] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.689074823] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.689114148] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.689516392] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.707979148] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.708350514] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.711451221] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.712762358] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.715883604] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.717353260] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.717399999] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.717955419] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_y8arjal6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.735359690] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.752307543] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.752981192] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.753122721] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.753137639] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.753147398] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.754931753] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.764345126] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408966.764965224] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408966.768961181] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408966.831056325] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 30307] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 30305] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408967.952895816] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408967.952950620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408969.067815810] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.287954 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408970.242220186] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.692100 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408973.093370250] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.842163 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408974.097789566] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.261560 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408975.182221851] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.693895 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408975.493127231] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408975.493178468] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408975.761371324] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408975.761406781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.525708663] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.180627 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.759130170] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.759166860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.759109321] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408977.759157713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408967.838581671] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408973.421448284] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:43109 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:43109 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:33] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.759627551] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.759664791] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.759674400] [moveit_1321688465.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.767103696] [moveit_1321688465.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.769480512] [moveit_1321688465.moveit.ros.rdf_loader]: Loaded robot model in 0.00218947 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.769521229] [moveit_1321688465.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.769534505] [moveit_1321688465.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408976.782308422] [moveit_1321688465.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.784827117] [moveit_1321688465.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.842885861] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.843029724] [moveit_1321688465.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844159581] [moveit_1321688465.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844705848] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844721188] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.844836857] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845239081] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845329123] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845741656] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.845753619] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.846418929] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408976.846909572] [moveit_1321688465.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408976.847130171] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778408976.847146892] [moveit_1321688465.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.026190048] [moveit_1321688465.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.027012000] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.028560699] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.028580166] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029042294] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029060158] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029087530] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029092890] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.029104763] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.030251060] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.032489603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.032505664] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.034504872] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.034520872] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.035246021] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.064593754] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.068137964] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.068329783] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.068354781] [moveit_1321688465.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408977.069257156] [moveit_1321688465.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:37] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:38] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408978.253042507] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:53 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:53 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:53 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408993.396324103] [moveit_1321688465.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:13 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_box_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py::test_pick_up_box_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 32.96s ========================= - - -12:30:09.05 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests -12:30:09.05 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-M0LXPH -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py::test_pick_up_sphere_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:29:49 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-36-208332-DESKTOP-HG3Q5KV-31089 (testutils.py:33) - -2026-05-10 12:29:49 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [31132] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [31133] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [31134] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-4]: process started with pid [31135] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-5]: process started with pid [31136] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778408976.532131849] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.538816220] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.541378107] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.543599518] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.543626990] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.543632180] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.543711736] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.712179565] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.715022676] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.715407007] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.715569145] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.723447589] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725043958] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725066681] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725097730] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725106938] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725180808] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408976.725238688] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408976.847930090] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408976.870043417] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.927368 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.100371637] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.100414398] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.103425614] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.123631408] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125002904] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125280957] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125347443] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.125374014] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.130716450] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.137757945] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.137803892] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.138929151] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.151276111] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.151567825] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.152423361] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.155483089] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.155519980] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.156208939] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.169217080] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.169756591] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.170325041] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.173431658] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.173467446] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.174155303] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.187297154] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.187556834] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.188103362] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.191662845] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.191708882] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.195586185] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.209227480] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.209505503] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.217730906] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.217775701] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.218087849] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.233220754] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.233461055] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.239684792] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.239724597] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.240073675] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.253174870] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.253446681] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.255739793] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.255776443] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.256341391] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-5] [INFO] [1778408977.271272187] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.271598111] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.396783636] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 31136] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.649546875] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.649609775] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.651136791] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.667450453] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.668743921] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.668804295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.673545174] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.674382585] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.677424104] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.678354858] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.678399773] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.678707297] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.691242857] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.691527402] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.692892381] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408977.704017285] [controller_manager]: Overrun might occur, Total time : 9843.524 us (Expected < 2000.000 us) --> Read time : 16.481 us, Update time : 9823.777 us (Switch time : 9768.661 us (Switch chained mode time : 0.431 us, perform mode change time : 14.798 us, Activation time : 9736.400 us, Deactivation time : 0.251 us)), Write time : 3.266 us (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.704036285] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.705518336] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.706683084] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.706724152] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.707090093] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.719565945] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.721550179] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.721975667] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.722040971] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.725085075] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.726296251] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.729448982] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.730892896] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.730927481] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.732069777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.751676412] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.751990223] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.754098475] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.755524634] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.756300214] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.759461649] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.760563018] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.760604086] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.761041777] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.777268744] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.777598496] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.781163922] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.782344650] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.785230733] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.786439094] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.786476435] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.786855476] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.800355021] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.813413029] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.813794809] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.813950234] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.813988898] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.814000470] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.815540661] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.823333378] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408977.824469884] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [spawner-4] [INFO] [1778408977.827313816] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 31135] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778408978.986796856] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31133] (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408979.256160972] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408979.256213061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408979.730218754] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.103215 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408980.410161770] [controller_manager]: Overrun might occur, Total time : 4012.994 us (Expected < 2000.000 us) --> Read time : 13.275 us, Update time : 3997.996 us, Write time : 1.723 us (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408981.194205176] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.089216 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408982.324902042] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.786393 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408985.170836516] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.720827 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408985.652126918] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408985.652184448] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408985.900973482] [controller_manager]: Overrun might occur, Total time : 2445.335 us (Expected < 2000.000 us) --> Read time : 14.387 us, Update time : 2429.245 us, Write time : 1.703 us (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408986.652330799] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408986.652378199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408987.652348365] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408987.652411194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [INFO] [1778408988.652280638] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [ros2_control_node-1] [WARN] [1778408988.652332526] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [1778408979.144732481] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] [INFO] [1778408989.644691271] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:29:49 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] * Running on http://127.0.0.1:45973 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] * Running on http://172.30.43.138:45973 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:44] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:45] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:45] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:45] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.030948253] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.031000402] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.031017494] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.038319258] [moveit_3024616344.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.040199040] [moveit_3024616344.moveit.ros.rdf_loader]: Loaded robot model in 0.0017393 seconds (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.040227244] [moveit_3024616344.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.040234498] [moveit_3024616344.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [WARN] [1778408988.051601396] [moveit_3024616344.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.054261235] [moveit_3024616344.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.105560063] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.105680061] [moveit_3024616344.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.106788242] [moveit_3024616344.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107389785] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107406727] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107530412] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.107941714] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.108032707] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.108699539] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.108713586] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.109143903] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.109605441] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [WARN] [1778408988.109888098] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [ERROR] [1778408988.109907495] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.327276701] [moveit_3024616344.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.327992271] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329105215] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329118861] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329488173] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329503632] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329535042] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329545071] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.329560129] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.330436805] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.332819794] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.332835243] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.335250452] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.335268376] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.336139982] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.369525108] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.373596495] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.373795773] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.373829197] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408988.374643979] [moveit_3024616344.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:49] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:29:49] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:29:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778408989.775074879] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:30:04 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:30:04 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] 2026-05-10 12:30:04 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:30:08 [ ERROR] [INFO] [1778409004.931996196] [moveit_3024616344.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -____________________ test_pick_up_sphere_by_position[ur5e] _____________________ - -start_processes = Urls(ros_domain_id='152', robot_url='http://0.0.0.0:45973', storage_url='http://127.0.0.1:44981') - - @pytest.mark.timeout(321) - def test_pick_up_sphere_by_position(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Sphere("Sphere1", 0.1) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Sphere1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-36-208332-DESKTOP-HG3Q5KV-31089 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [31132] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [31133] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [31134] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [31135] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [31136] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408976.532131849] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.538816220] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.541378107] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.543599518] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.543626990] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.543632180] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.543711736] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.712179565] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.715022676] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.715407007] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.715569145] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.723447589] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725043958] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725066681] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725097730] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725106938] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725180808] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408976.725238688] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408976.847930090] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408976.870043417] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.927368 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.100371637] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.100414398] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.103425614] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.123631408] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125002904] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125280957] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125347443] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.125374014] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.130716450] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.137757945] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.137803892] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.138929151] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.151276111] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.151567825] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.152423361] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.155483089] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.155519980] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.156208939] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.169217080] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.169756591] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.170325041] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.173431658] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.173467446] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.174155303] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.187297154] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.187556834] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.188103362] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.191662845] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.191708882] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.195586185] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.209227480] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.209505503] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.217730906] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.217775701] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.218087849] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.233220754] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.233461055] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.239684792] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.239724597] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.240073675] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.253174870] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.253446681] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.255739793] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.255776443] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.256341391] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408977.271272187] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.271598111] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.396783636] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 31136] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.649546875] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.649609775] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.651136791] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.667450453] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.668743921] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.668804295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.673545174] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.674382585] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.677424104] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.678354858] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.678399773] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.678707297] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.691242857] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.691527402] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.692892381] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408977.704017285] [controller_manager]: Overrun might occur, Total time : 9843.524 us (Expected < 2000.000 us) --> Read time : 16.481 us, Update time : 9823.777 us (Switch time : 9768.661 us (Switch chained mode time : 0.431 us, perform mode change time : 14.798 us, Activation time : 9736.400 us, Deactivation time : 0.251 us)), Write time : 3.266 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.704036285] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.705518336] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.706683084] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.706724152] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.707090093] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.719565945] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.721550179] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.721975667] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.722040971] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.725085075] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.726296251] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.729448982] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.730892896] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.730927481] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.732069777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.751676412] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.751990223] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.754098475] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.755524634] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.756300214] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.759461649] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.760563018] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.760604086] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.761041777] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.777268744] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.777598496] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.781163922] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.782344650] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.785230733] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.786439094] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.786476435] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.786855476] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_0n4sq4 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.800355021] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.813413029] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.813794809] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.813950234] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.813988898] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.814000470] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.815540661] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.823333378] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408977.824469884] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408977.827313816] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 31135] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408978.986796856] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31133] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408979.256160972] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408979.256213061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408979.730218754] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.103215 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408980.410161770] [controller_manager]: Overrun might occur, Total time : 4012.994 us (Expected < 2000.000 us) --> Read time : 13.275 us, Update time : 3997.996 us, Write time : 1.723 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408981.194205176] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.089216 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408982.324902042] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.786393 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408985.170836516] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.720827 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408985.652126918] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408985.652184448] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408985.900973482] [controller_manager]: Overrun might occur, Total time : 2445.335 us (Expected < 2000.000 us) --> Read time : 14.387 us, Update time : 2429.245 us, Write time : 1.703 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408986.652330799] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408986.652378199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408987.652348365] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408987.652411194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408988.652280638] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408988.652332526] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408979.144732481] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408989.644691271] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:45973 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:45973 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:44] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:45] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:45] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:45] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.030948253] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.031000402] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.031017494] [moveit_3024616344.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.038319258] [moveit_3024616344.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.040199040] [moveit_3024616344.moveit.ros.rdf_loader]: Loaded robot model in 0.0017393 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.040227244] [moveit_3024616344.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.040234498] [moveit_3024616344.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408988.051601396] [moveit_3024616344.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.054261235] [moveit_3024616344.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.105560063] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.105680061] [moveit_3024616344.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.106788242] [moveit_3024616344.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107389785] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107406727] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107530412] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.107941714] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.108032707] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.108699539] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.108713586] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.109143903] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.109605441] [moveit_3024616344.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778408988.109888098] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778408988.109907495] [moveit_3024616344.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.327276701] [moveit_3024616344.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.327992271] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329105215] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329118861] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329488173] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329503632] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329535042] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329545071] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.329560129] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.330436805] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.332819794] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.332835243] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.335250452] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.335268376] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.336139982] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.369525108] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.373596495] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.373795773] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.373829197] [moveit_3024616344.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408988.374643979] [moveit_3024616344.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:49] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:29:49] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:29:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408989.775074879] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409004.931996196] [moveit_3024616344.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:13 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_sphere_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py::test_pick_up_sphere_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 33.05s ========================= - - -12:30:34.98 [INFO] Long running tasks: - 65.52s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests -12:30:39.57 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests -12:30:39.58 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-EPF00Y -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py::test_overhead_box[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:30:21 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-56-696264-DESKTOP-HG3Q5KV-31785 (testutils.py:33) - -2026-05-10 12:30:21 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [31792] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [31793] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [31794] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-4]: process started with pid [31795] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-5]: process started with pid [31796] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778408997.075826168] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.091213456] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.094959849] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.098099809] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.098131529] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.098374862] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408997.098521325] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.262584014] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.265282091] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.265616988] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.265666091] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.274549632] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276076593] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276115477] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276141877] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276148008] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276222410] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.276282724] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.368266884] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.620767270] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.620818156] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.624132694] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408997.630046397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.267420 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.644042796] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645269107] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645444440] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645480598] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.645502330] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.650400321] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.658278886] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.658322038] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.659219563] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.671871063] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.672163334] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.672939964] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.675891227] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.675925212] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.676774911] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.689813387] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.690126871] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.690625950] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.691948232] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.691984872] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.692584051] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.704144933] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.704432229] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.704991502] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.708492905] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.708525026] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.712287966] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.725961027] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.726241034] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.734424809] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.734466408] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.734794271] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.747960768] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.748244317] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.754492033] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.754529464] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.754827961] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.768213194] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.768487131] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.772488984] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.772526055] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.773097390] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-5] [INFO] [1778408997.787756119] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408997.788031146] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408997.931075754] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 31796] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.184148316] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.184245441] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.185784900] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.202030494] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.203319769] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.203383029] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.208649421] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.209091591] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.212127350] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.213036537] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.213067636] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.213405268] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.227737962] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.228018956] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.229516351] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408998.240572529] [controller_manager]: Overrun might occur, Total time : 9714.758 us (Expected < 2000.000 us) --> Read time : 13.525 us, Update time : 9699.139 us (Switch time : 9654.174 us (Switch chained mode time : 0.401 us, perform mode change time : 10.681 us, Activation time : 9628.846 us, Deactivation time : 0.170 us)), Write time : 2.094 us (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.240593855] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.241862610] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.242895367] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.242934972] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.243380749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.256437498] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.257842874] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.258110317] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.258166133] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.259704411] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.260967801] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.264109406] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.265285346] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.265320413] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.266422096] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.283771757] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.284039070] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.286017098] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.287665035] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.288977478] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.291895062] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.292986010] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.293027339] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.293320586] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.308005730] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.308380031] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.311745756] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.313007394] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.316009398] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.317300651] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.317331079] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.317638423] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.332088124] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.346500666] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347027667] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347199564] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347259548] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.347276029] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.348974706] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.355649955] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408998.357159448] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [spawner-4] [INFO] [1778408998.360175038] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 31795] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778408999.456590096] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31793] (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.753389224] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778408999.753434510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.685824435] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.045749 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.734163094] [controller_manager]: Overrun might occur, Total time : 3322.173 us (Expected < 2000.000 us) --> Read time : 15.990 us, Update time : 3304.570 us, Write time : 1.613 us (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409002.816354397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.575370 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409004.955654364] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.875678 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409005.848364594] [controller_manager]: Overrun might occur, Total time : 3533.003 us (Expected < 2000.000 us) --> Read time : 15.079 us, Update time : 3516.101 us, Write time : 1.823 us (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409006.302054930] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.302105346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.714830477] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.051360 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409007.302447423] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409007.302524730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409007.825926331] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.147565 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409008.302560631] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409008.302626716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.269975539] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.196803 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409009.305686925] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.305720328] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.302340950] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.302403238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.403583042] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.954689770] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.302293714] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.302333429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.378954189] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.175212 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.522961023] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.523028521] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409012.302401106] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.302455079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.169982343] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.203317 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.302385581] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.302439824] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.397415855] [controller_manager]: Overrun might occur, Total time : 2555.311 us (Expected < 2000.000 us) --> Read time : 196.584 us, Update time : 2355.250 us, Write time : 3.477 us (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.302399895] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.302457845] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.550287817] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.508870 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409015.302308050] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409015.302361051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409016.302368400] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.302422954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.546304253] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.525577 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.697812521] [controller_manager]: Overrun might occur, Total time : 2473.369 us (Expected < 2000.000 us) --> Read time : 19.567 us, Update time : 2451.929 us, Write time : 1.873 us (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409017.302685362] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.302746348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.681561355] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.782258 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409018.302889468] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409018.302947558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.147569303] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.790026 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409019.302348651] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.302404777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.439028730] [controller_manager]: Overrun might occur, Total time : 5949.926 us (Expected < 2000.000 us) --> Read time : 441.348 us, Update time : 5504.240 us, Write time : 4.338 us (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409020.306165682] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.306223072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.344654875] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.876119 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409021.302523291] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.302568958] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.380114272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.335556 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [1778408999.633011969] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] [INFO] [1778409001.558812388] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:30:21 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:04 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] * Running on http://127.0.0.1:49437 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] * Running on http://172.30.43.138:49437 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:05] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.939037878] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.939095227] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.939106769] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.945591020] [moveit_1524511032.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.947918924] [moveit_1524511032.moveit.ros.rdf_loader]: Loaded robot model in 0.00216334 seconds (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.947957707] [moveit_1524511032.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.947968157] [moveit_1524511032.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [WARN] [1778409008.960680858] [moveit_1524511032.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409008.963945891] [moveit_1524511032.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.022826706] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.022995417] [moveit_1524511032.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.023988775] [moveit_1524511032.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.024694626] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.024712009] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.024835313] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.025359129] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.025448890] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.026187768] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.026205782] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.026678390] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.027105001] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [WARN] [1778409009.027304901] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [ERROR] [1778409009.027315751] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.071683044] [moveit_1524511032.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.072617791] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074270126] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074288942] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074693811] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074711685] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074737855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074742363] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.074751851] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.076117260] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.079019074] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.079033892] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.084554161] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.084576053] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.085796331] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.447130590] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.450460498] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.450637975] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.450667070] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409009.451601651] [moveit_1524511032.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:10] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:10] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:11 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632727433] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632852721] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632862559] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.632880694] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [WARN] [1778409011.632980183] [moveit_1524511032.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.633454546] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409011.633558343] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [ERROR] [1778409021.634133598] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [ERROR] [1778409021.634272271] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:21 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:21] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409021.876359631] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:36 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:36 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] 2026-05-10 12:30:36 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:30:39 [ ERROR] [INFO] [1778409037.016289503] [moveit_1524511032.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -___________________________ test_overhead_box[ur5e] ____________________________ - -start_processes = Urls(ros_domain_id='217', robot_url='http://0.0.0.0:49437', storage_url='http://127.0.0.1:34855') - - @pytest.mark.timeout(400) - def test_overhead_box(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -> ot.move_to_pose("", start_pose, 0.3, safe=False) - -src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-56-696264-DESKTOP-HG3Q5KV-31785 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [31792] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [31793] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [31794] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [31795] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [31796] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408997.075826168] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.091213456] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.094959849] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.098099809] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.098131529] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.098374862] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408997.098521325] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.262584014] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.265282091] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.265616988] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.265666091] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.274549632] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276076593] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276115477] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276141877] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276148008] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276222410] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.276282724] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.368266884] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.620767270] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.620818156] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.624132694] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408997.630046397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.267420 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.644042796] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645269107] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645444440] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645480598] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.645502330] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.650400321] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.658278886] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.658322038] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.659219563] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.671871063] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.672163334] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.672939964] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.675891227] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.675925212] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.676774911] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.689813387] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.690126871] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.690625950] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.691948232] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.691984872] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.692584051] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.704144933] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.704432229] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.704991502] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.708492905] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.708525026] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.712287966] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.725961027] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.726241034] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.734424809] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.734466408] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.734794271] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.747960768] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.748244317] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.754492033] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.754529464] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.754827961] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.768213194] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.768487131] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.772488984] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.772526055] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.773097390] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408997.787756119] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408997.788031146] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408997.931075754] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 31796] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.184148316] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.184245441] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.185784900] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.202030494] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.203319769] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.203383029] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.208649421] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.209091591] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.212127350] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.213036537] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.213067636] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.213405268] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.227737962] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.228018956] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.229516351] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408998.240572529] [controller_manager]: Overrun might occur, Total time : 9714.758 us (Expected < 2000.000 us) --> Read time : 13.525 us, Update time : 9699.139 us (Switch time : 9654.174 us (Switch chained mode time : 0.401 us, perform mode change time : 10.681 us, Activation time : 9628.846 us, Deactivation time : 0.170 us)), Write time : 2.094 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.240593855] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.241862610] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.242895367] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.242934972] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.243380749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.256437498] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.257842874] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.258110317] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.258166133] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.259704411] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.260967801] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.264109406] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.265285346] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.265320413] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.266422096] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.283771757] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.284039070] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.286017098] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.287665035] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.288977478] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.291895062] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.292986010] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.293027339] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.293320586] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.308005730] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.308380031] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.311745756] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.313007394] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.316009398] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.317300651] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.317331079] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.317638423] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7gbhrpl5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.332088124] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.346500666] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347027667] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347199564] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347259548] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.347276029] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.348974706] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.355649955] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408998.357159448] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408998.360175038] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 31795] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408999.456590096] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31793] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.753389224] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408999.753434510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.685824435] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.045749 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.734163094] [controller_manager]: Overrun might occur, Total time : 3322.173 us (Expected < 2000.000 us) --> Read time : 15.990 us, Update time : 3304.570 us, Write time : 1.613 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409002.816354397] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.575370 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409004.955654364] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.875678 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409005.848364594] [controller_manager]: Overrun might occur, Total time : 3533.003 us (Expected < 2000.000 us) --> Read time : 15.079 us, Update time : 3516.101 us, Write time : 1.823 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409006.302054930] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.302105346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.714830477] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.051360 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409007.302447423] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409007.302524730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409007.825926331] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.147565 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409008.302560631] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409008.302626716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.269975539] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.196803 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409009.305686925] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.305720328] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.302340950] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.302403238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.403583042] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.954689770] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.302293714] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.302333429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.378954189] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.175212 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.522961023] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.523028521] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409012.302401106] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.302455079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.169982343] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.203317 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.302385581] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.302439824] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.397415855] [controller_manager]: Overrun might occur, Total time : 2555.311 us (Expected < 2000.000 us) --> Read time : 196.584 us, Update time : 2355.250 us, Write time : 3.477 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.302399895] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.302457845] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.550287817] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.508870 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409015.302308050] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409015.302361051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409016.302368400] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.302422954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.546304253] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.525577 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.697812521] [controller_manager]: Overrun might occur, Total time : 2473.369 us (Expected < 2000.000 us) --> Read time : 19.567 us, Update time : 2451.929 us, Write time : 1.873 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409017.302685362] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.302746348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.681561355] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.782258 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409018.302889468] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409018.302947558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.147569303] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.790026 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409019.302348651] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.302404777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.439028730] [controller_manager]: Overrun might occur, Total time : 5949.926 us (Expected < 2000.000 us) --> Read time : 441.348 us, Update time : 5504.240 us, Write time : 4.338 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409020.306165682] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.306223072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.344654875] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.876119 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409021.302523291] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.302568958] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.380114272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.335556 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778408999.633011969] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409001.558812388] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:04 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:49437 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:49437 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:05] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.939037878] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.939095227] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.939106769] [moveit_1524511032.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.945591020] [moveit_1524511032.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.947918924] [moveit_1524511032.moveit.ros.rdf_loader]: Loaded robot model in 0.00216334 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.947957707] [moveit_1524511032.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.947968157] [moveit_1524511032.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409008.960680858] [moveit_1524511032.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409008.963945891] [moveit_1524511032.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.022826706] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.022995417] [moveit_1524511032.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.023988775] [moveit_1524511032.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.024694626] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.024712009] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.024835313] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.025359129] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.025448890] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.026187768] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.026205782] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.026678390] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.027105001] [moveit_1524511032.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409009.027304901] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409009.027315751] [moveit_1524511032.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.071683044] [moveit_1524511032.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.072617791] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074270126] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074288942] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074693811] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074711685] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074737855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074742363] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.074751851] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.076117260] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.079019074] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.079033892] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.084554161] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.084576053] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.085796331] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.447130590] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.450460498] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.450637975] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.450667070] [moveit_1524511032.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409009.451601651] [moveit_1524511032.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:10] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:10] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632727433] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632852721] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632862559] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.632880694] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409011.632980183] [moveit_1524511032.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.633454546] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.633558343] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409021.634133598] [moveit_1524511032.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409021.634272271] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:21] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409021.876359631] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409037.016289503] [moveit_1524511032.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:12 - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_overhead_obstacle_box.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py::test_overhead_box[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed -======================== 1 failed, 1 warning in 43.16s ========================= - - -12:30:49.05 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests -12:30:49.05 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-iQVxGl -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py::test_side_cylinder[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:30:31 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-58-744636-DESKTOP-HG3Q5KV-31946 (testutils.py:33) - -2026-05-10 12:30:31 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [31953] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [31954] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [31955] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-4]: process started with pid [31956] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-5]: process started with pid [31957] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [robot_state_publisher-2] [INFO] [1778408999.018787519] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.026245059] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.029076339] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.032108079] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.032148777] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.032159577] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778408999.032281970] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.196684494] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.199575428] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.199984425] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.200050791] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.208610119] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209898112] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209938097] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209967774] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.209973214] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.210037316] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.210087571] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.301775740] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [robot_state_publisher-2] [INFO] [1778408999.456604157] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.553738109] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.553806088] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.556827178] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31954] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.573988176] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575031854] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575199993] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575231503] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.575254717] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.579660600] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.585689029] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.585726080] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.586587026] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.597469934] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.597743735] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.598429628] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.599612370] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.599652957] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.600197502] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.611847644] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.612132265] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.612712818] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.615895645] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.615928488] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.616454858] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.627547190] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.627791981] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.628290217] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.629685027] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.629717138] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.633376186] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.643816469] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.644074038] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.652029134] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.652067867] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.652345685] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.663591523] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.663867517] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.668057479] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.668092466] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.668424046] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.679879923] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.680177789] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.684348424] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.684383240] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.684907696] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-5] [INFO] [1778408999.700323563] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778408999.700755238] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778408999.808631471] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 31957] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.061036841] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.061084651] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.062516612] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.075693365] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.076606064] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.076664144] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.082076244] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.082822902] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.085608175] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.086504392] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.086540050] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.086900234] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.099527203] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.099845066] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.101427227] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409000.110469304] [controller_manager]: Overrun might occur, Total time : 7871.879 us (Expected < 2000.000 us) --> Read time : 11.733 us, Update time : 7857.922 us (Switch time : 7817.054 us (Switch chained mode time : 0.341 us, perform mode change time : 10.500 us, Activation time : 7790.223 us, Deactivation time : 0.140 us)), Write time : 2.224 us (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409000.110536271] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.015016 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.110480034] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.113878732] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.114791201] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.114820857] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.115244147] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.126690731] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.129627393] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.129925324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.129982412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.133343131] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.134706953] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.137539772] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.138636650] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.138673190] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.139745577] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.155429869] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.155692594] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.157674609] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.159416796] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.160773183] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.163671068] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.164707834] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.164745896] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.164981123] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.177589622] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.177909223] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.181346028] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.182723124] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.185585753] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.186685964] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.186737602] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.186996444] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.199418583] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.211744318] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212081008] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212252384] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212290206] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.212301057] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.213704162] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.219737311] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409000.220911061] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [spawner-4] [INFO] [1778409000.223906728] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 31956] (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.685739664] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.218930 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409001.902787781] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409001.902865669] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409002.818200709] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.680145 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409004.955824146] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.303502 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.712170964] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.650100 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409006.768863972] [controller_manager]: Overrun might occur, Total time : 5764.203 us (Expected < 2000.000 us) --> Read time : 28.094 us, Update time : 5734.216 us, Write time : 1.893 us (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409007.825452135] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930940 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409008.481078451] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409008.481134999] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.269893342] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.372468 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409009.481409649] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409009.481466267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.481271029] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.481320062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.481317806] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.481366458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.879705361] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.184547 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.205388461] [controller_manager]: Overrun might occur, Total time : 2496.594 us (Expected < 2000.000 us) --> Read time : 44.880 us, Update time : 2447.286 us, Write time : 4.428 us (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409012.247123489] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409012.481498403] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.481545182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.798347172] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.073235669] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.714985 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.354328407] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.354388000] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.393956591] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.394024249] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.398033098] [controller_manager]: Overrun might occur, Total time : 3429.098 us (Expected < 2000.000 us) --> Read time : 23.094 us, Update time : 3402.477 us, Write time : 3.527 us (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.481509953] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.481567272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.481203599] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.481258443] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409015.342309325] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.788460 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409015.481320373] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409015.481687260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409016.016631026] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409016.481494817] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.481538019] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.545472938] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.952314 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409017.481322328] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.481370599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409018.481496856] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409018.481550728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.420874128] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.353224 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.437336565] [controller_manager]: Overrun might occur, Total time : 3371.067 us (Expected < 2000.000 us) --> Read time : 41.910 us, Update time : 3323.867 us, Write time : 5.290 us (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409019.481814993] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.481895636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409020.481716499] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.481756555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.502572992] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.051858 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.626193085] [controller_manager]: Overrun might occur, Total time : 3608.958 us (Expected < 2000.000 us) --> Read time : 19.236 us, Update time : 3587.227 us, Write time : 2.495 us (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409021.481374444] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.481416384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409022.481276873] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409022.481328210] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409023.481120312] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409023.481172571] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409024.481344627] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409024.481399341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409025.481388742] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409025.481445921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409026.481290626] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409026.481346903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409027.481342380] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409027.481401342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409028.481265890] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409028.481325343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409029.481288400] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409029.481342152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [INFO] [1778409030.481355747] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [ros2_control_node-1] [WARN] [1778409030.481431941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [1778409001.780580619] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] [INFO] [1778409013.066125273] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:30:31 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] * Running on http://127.0.0.1:51469 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] * Running on http://172.30.43.138:51469 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:07] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.717336898] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.717395339] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.717409035] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.721851099] [moveit_3334433294.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.724119631] [moveit_3334433294.moveit.ros.rdf_loader]: Loaded robot model in 0.00211411 seconds (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.724149488] [moveit_3334433294.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.724157253] [moveit_3334433294.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [WARN] [1778409010.737205550] [moveit_3334433294.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.740801880] [moveit_3334433294.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.801893169] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.802069139] [moveit_3334433294.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803152187] [moveit_3334433294.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803697854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803716148] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.803879303] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804243665] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804340619] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804882854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.804899907] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.805366068] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409010.805799711] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [WARN] [1778409010.806045368] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [ERROR] [1778409010.806063823] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.782611224] [moveit_3334433294.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.783615122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785044428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785058234] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785345721] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785357453] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785379745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785383853] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.785393582] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.786151592] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.788536603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.788549828] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.790883823] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.790900395] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.791659397] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.813791671] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.816967616] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.817127911] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.817160843] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409011.817864781] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:12] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:12] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365168505] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365305395] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365325083] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365349299] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [WARN] [1778409013.365463405] [moveit_3334433294.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.365933469] [moveit_3334433294.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.366059839] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.383303483] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.384495418] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.384854039] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Executing plan (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392239799] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392270547] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392303129] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392348034] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392377410] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.392969516] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.393005053] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.393028718] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.393170878] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.395063093] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409013.395082019] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409016.045000726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409016.050990843] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:31 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self._wait_for_pose_reached(target_abs, tolerance=0.01) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] raise UrGeneral("Target pose was not reached.") (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Target pose was not reached. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:31] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409031.265057490] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:46 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:46 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] 2026-05-10 12:30:46 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:30:48 [ ERROR] [INFO] [1778409046.402290002] [moveit_3334433294.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -___________________________ test_side_cylinder[ur5e] ___________________________ - -start_processes = Urls(ros_domain_id='126', robot_url='http://0.0.0.0:51469', storage_url='http://127.0.0.1:45425') - - @pytest.mark.timeout(400) - def test_side_cylinder(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -> ot.move_to_pose("", start_pose, 0.3, safe=False) - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not reached. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-29-58-744636-DESKTOP-HG3Q5KV-31946 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [31953] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [31954] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [31955] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [31956] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [31957] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408999.018787519] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.026245059] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.029076339] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.032108079] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.032148777] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.032159577] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778408999.032281970] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.196684494] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.199575428] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.199984425] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.200050791] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.208610119] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209898112] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209938097] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209967774] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.209973214] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.210037316] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.210087571] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.301775740] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778408999.456604157] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.553738109] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.553806088] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.556827178] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 31954] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.573988176] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575031854] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575199993] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575231503] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.575254717] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.579660600] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.585689029] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.585726080] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.586587026] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.597469934] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.597743735] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.598429628] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.599612370] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.599652957] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.600197502] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.611847644] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.612132265] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.612712818] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.615895645] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.615928488] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.616454858] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.627547190] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.627791981] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.628290217] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.629685027] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.629717138] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.633376186] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.643816469] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.644074038] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.652029134] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.652067867] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.652345685] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.663591523] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.663867517] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.668057479] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.668092466] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.668424046] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.679879923] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.680177789] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.684348424] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.684383240] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.684907696] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778408999.700323563] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778408999.700755238] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778408999.808631471] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 31957] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.061036841] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.061084651] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.062516612] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.075693365] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.076606064] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.076664144] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.082076244] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.082822902] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.085608175] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.086504392] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.086540050] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.086900234] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.099527203] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.099845066] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.101427227] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409000.110469304] [controller_manager]: Overrun might occur, Total time : 7871.879 us (Expected < 2000.000 us) --> Read time : 11.733 us, Update time : 7857.922 us (Switch time : 7817.054 us (Switch chained mode time : 0.341 us, perform mode change time : 10.500 us, Activation time : 7790.223 us, Deactivation time : 0.140 us)), Write time : 2.224 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409000.110536271] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.015016 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.110480034] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.113878732] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.114791201] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.114820857] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.115244147] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.126690731] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.129627393] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.129925324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.129982412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.133343131] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.134706953] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.137539772] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.138636650] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.138673190] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.139745577] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.155429869] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.155692594] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.157674609] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.159416796] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.160773183] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.163671068] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.164707834] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.164745896] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.164981123] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.177589622] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.177909223] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.181346028] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.182723124] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.185585753] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.186685964] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.186737602] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.186996444] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ca6eoapf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.199418583] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.211744318] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212081008] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212252384] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212290206] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.212301057] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.213704162] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.219737311] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409000.220911061] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409000.223906728] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 31956] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.685739664] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.218930 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409001.902787781] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409001.902865669] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409002.818200709] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.680145 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409004.955824146] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.303502 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.712170964] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.650100 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409006.768863972] [controller_manager]: Overrun might occur, Total time : 5764.203 us (Expected < 2000.000 us) --> Read time : 28.094 us, Update time : 5734.216 us, Write time : 1.893 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409007.825452135] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930940 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409008.481078451] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409008.481134999] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.269893342] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.372468 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409009.481409649] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409009.481466267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.481271029] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.481320062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.481317806] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.481366458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.879705361] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.184547 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.205388461] [controller_manager]: Overrun might occur, Total time : 2496.594 us (Expected < 2000.000 us) --> Read time : 44.880 us, Update time : 2447.286 us, Write time : 4.428 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409012.247123489] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409012.481498403] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.481545182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.798347172] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.073235669] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.714985 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.354328407] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.354388000] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.393956591] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.394024249] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.398033098] [controller_manager]: Overrun might occur, Total time : 3429.098 us (Expected < 2000.000 us) --> Read time : 23.094 us, Update time : 3402.477 us, Write time : 3.527 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.481509953] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.481567272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.481203599] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.481258443] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409015.342309325] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.788460 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409015.481320373] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409015.481687260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409016.016631026] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409016.481494817] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.481538019] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.545472938] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.952314 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409017.481322328] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.481370599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409018.481496856] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409018.481550728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.420874128] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.353224 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.437336565] [controller_manager]: Overrun might occur, Total time : 3371.067 us (Expected < 2000.000 us) --> Read time : 41.910 us, Update time : 3323.867 us, Write time : 5.290 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409019.481814993] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.481895636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409020.481716499] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.481756555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.502572992] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.051858 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.626193085] [controller_manager]: Overrun might occur, Total time : 3608.958 us (Expected < 2000.000 us) --> Read time : 19.236 us, Update time : 3587.227 us, Write time : 2.495 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409021.481374444] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.481416384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409022.481276873] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409022.481328210] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409023.481120312] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409023.481172571] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409024.481344627] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409024.481399341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409025.481388742] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409025.481445921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409026.481290626] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409026.481346903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409027.481342380] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409027.481401342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409028.481265890] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409028.481325343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409029.481288400] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409029.481342152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409030.481355747] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409030.481431941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409001.780580619] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.066125273] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:51469 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:51469 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:07] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.717336898] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.717395339] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.717409035] [moveit_3334433294.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.721851099] [moveit_3334433294.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.724119631] [moveit_3334433294.moveit.ros.rdf_loader]: Loaded robot model in 0.00211411 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.724149488] [moveit_3334433294.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.724157253] [moveit_3334433294.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409010.737205550] [moveit_3334433294.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.740801880] [moveit_3334433294.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.801893169] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.802069139] [moveit_3334433294.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803152187] [moveit_3334433294.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803697854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803716148] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.803879303] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804243665] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804340619] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804882854] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.804899907] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.805366068] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409010.805799711] [moveit_3334433294.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409010.806045368] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409010.806063823] [moveit_3334433294.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.782611224] [moveit_3334433294.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.783615122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785044428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785058234] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785345721] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785357453] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785379745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785383853] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.785393582] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.786151592] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.788536603] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.788549828] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.790883823] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.790900395] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.791659397] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.813791671] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.816967616] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.817127911] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.817160843] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409011.817864781] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:12] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:12] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365168505] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365305395] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365325083] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365349299] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409013.365463405] [moveit_3334433294.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.365933469] [moveit_3334433294.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.366059839] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.383303483] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.384495418] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.384854039] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392239799] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392270547] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392303129] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392348034] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392377410] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.392969516] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.393005053] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.393028718] [moveit_3334433294.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.393170878] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.395063093] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.395082019] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409016.045000726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409016.050990843] [moveit_3334433294.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:31 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._wait_for_pose_reached(target_abs, tolerance=0.01) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Target pose was not reached.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Target pose was not reached. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:31] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409031.265057490] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:46 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:46 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:46 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409046.402290002] [moveit_3334433294.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:12 - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_cylinder.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py::test_side_cylinder[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not ... -======================== 1 failed, 1 warning in 50.51s ========================= - - -12:30:52.85 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests -12:30:55.84 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests -12:30:55.85 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-EUAiHN -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py::test_overhead_cylinder[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:30:37 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-10-302933-DESKTOP-HG3Q5KV-32555 (testutils.py:33) - -2026-05-10 12:30:37 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [32586] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [32587] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [32588] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-4]: process started with pid [32589] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-5]: process started with pid [32590] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [robot_state_publisher-2] [INFO] [1778409010.686719380] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.696301908] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.700265705] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.702719828] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.702736941] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.702741389] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409010.702980684] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.860846613] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.864053567] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.864502750] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.864580218] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.873333134] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874756919] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874796634] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874828325] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874835869] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874915590] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409010.874969693] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409010.998711197] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.251069928] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.251140081] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.254010582] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.266969910] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.268098811] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.268206015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.277044884] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.277510007] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.280816815] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.281804592] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.281845991] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.287064242] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.298641395] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.299003282] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.299893428] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.311253639] [controller_manager]: Overrun might occur, Total time : 9947.300 us (Expected < 2000.000 us) --> Read time : 12.914 us, Update time : 9932.152 us (Switch time : 9888.690 us (Switch chained mode time : 0.691 us, perform mode change time : 10.560 us, Activation time : 9862.320 us, Deactivation time : 0.161 us)), Write time : 2.234 us (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409011.311326467] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.104770 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.311267836] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.314854471] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.315962932] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.316025521] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.316364194] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.326678598] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.328770634] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.329087426] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.329165674] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.332007584] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.333449334] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.336709729] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.337830128] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.337870545] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.338996394] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.356977304] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.357344462] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.359973292] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.362518594] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.363474275] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.366577315] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.367953771] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.367985101] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.368252769] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.385557587] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.385984272] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.390264464] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.391449962] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.394649252] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.396071629] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.396112647] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.396503649] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.411317257] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.425152336] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425514362] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425725223] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425781730] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.425809663] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.428261043] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.436395799] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409011.437580595] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-4] [INFO] [1778409011.440882559] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 32589] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409011.788984488] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409012.208551170] [controller_manager]: Overrun might occur, Total time : 3249.815 us (Expected < 2000.000 us) --> Read time : 2968.210 us, Update time : 278.930 us, Write time : 2.675 us (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [robot_state_publisher-2] [INFO] [1778409013.066150608] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.071854915] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.633579 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 32587] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409013.424310066] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409013.424372264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.543200365] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.543282021] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.543451092] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409014.545886393] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.664946 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.564706333] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566258499] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566402332] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566437689] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.566453890] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.568179028] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.574716419] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.574742418] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.576399747] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.592742355] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.593038142] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.593972316] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.596952846] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.596975629] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.597811373] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.614667065] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.615405543] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.616021543] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.618713037] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.618735600] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.619651916] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.634685001] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.635574436] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.636260619] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.639338713] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.639358380] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.639811862] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.653444801] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.654268006] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.667567853] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.667605865] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.667918980] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.683011728] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.684173345] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.691723277] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.691764375] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.692469544] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.708776554] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.710233772] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.713331654] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.713365809] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.713633584] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [spawner-5] [INFO] [1778409014.729014625] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409014.729872685] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 32590] (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409016.546436975] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.215869 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409017.669720656] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.499179 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409019.054380047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.158400 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.348171367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.950251 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409020.937156000] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409020.937210133] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.379388443] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.167117 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409021.936779045] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409021.936830403] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409022.936727011] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409022.936786224] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409023.936929120] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409023.936983644] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409024.936780949] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409024.936831305] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409025.398858931] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409025.937238058] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409025.937301318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409025.949971093] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409026.518345970] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409026.518418778] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409026.936841666] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409026.936884748] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409027.936869374] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409027.936929287] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409028.936981644] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409028.937193731] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409029.936771943] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409029.936830324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409030.936850265] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409030.936925698] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409031.936824906] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409031.936883608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409032.936864678] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409032.936918370] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409033.936777707] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409033.936829736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409034.936901721] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409034.936955754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409035.936853648] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409035.936907590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [INFO] [1778409036.936811406] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [ros2_control_node-1] [WARN] [1778409036.936864317] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [1778409013.315860179] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] [INFO] [1778409037.213346839] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:30:37 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:19 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] * Running on http://127.0.0.1:36751 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] * Running on http://172.30.43.138:36751 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:20] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.245905266] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.245968446] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.245982232] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.252387979] [moveit_2882792308.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.254749016] [moveit_2882792308.moveit.ros.rdf_loader]: Loaded robot model in 0.00209667 seconds (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.254777200] [moveit_2882792308.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.254784704] [moveit_2882792308.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [WARN] [1778409023.266574569] [moveit_2882792308.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.269300339] [moveit_2882792308.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.325565849] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.325708581] [moveit_2882792308.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.326718009] [moveit_2882792308.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327277251] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327291939] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327398286] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327900061] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.327988309] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.328585322] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.328598828] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.329053752] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.329505531] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [WARN] [1778409023.329734806] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [ERROR] [1778409023.329752169] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.488595075] [moveit_2882792308.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.489266010] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490406918] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490419261] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490752514] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490766451] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490790015] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490795185] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.490810935] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.491639349] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.493935141] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.493950330] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.496156492] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.496170008] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.496809443] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.526223248] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.530075442] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.530245956] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.530271745] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409023.531111836] [moveit_2882792308.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:25] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:25] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:26 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547103323] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547236236] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547248579] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547267606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [WARN] [1778409026.547376312] [moveit_2882792308.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547832829] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409026.547954100] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [ERROR] [1778409036.548437353] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [ERROR] [1778409036.548540139] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:36 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:37] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409037.342777967] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:52 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:52 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] 2026-05-10 12:30:52 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:30:55 [ ERROR] [INFO] [1778409052.485310461] [moveit_2882792308.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_________________________ test_overhead_cylinder[ur5e] _________________________ - -start_processes = Urls(ros_domain_id='187', robot_url='http://0.0.0.0:36751', storage_url='http://127.0.0.1:54545') - - @pytest.mark.timeout(400) - def test_overhead_cylinder(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -> ot.move_to_pose("", start_pose, 0.3, safe=False) - -src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-10-302933-DESKTOP-HG3Q5KV-32555 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [32586] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [32587] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [32588] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [32589] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [32590] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409010.686719380] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.696301908] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.700265705] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.702719828] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.702736941] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.702741389] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409010.702980684] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.860846613] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.864053567] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.864502750] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.864580218] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.873333134] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874756919] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874796634] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874828325] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874835869] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874915590] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409010.874969693] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409010.998711197] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.251069928] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.251140081] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.254010582] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.266969910] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.268098811] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.268206015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.277044884] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.277510007] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.280816815] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.281804592] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.281845991] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.287064242] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.298641395] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.299003282] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.299893428] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.311253639] [controller_manager]: Overrun might occur, Total time : 9947.300 us (Expected < 2000.000 us) --> Read time : 12.914 us, Update time : 9932.152 us (Switch time : 9888.690 us (Switch chained mode time : 0.691 us, perform mode change time : 10.560 us, Activation time : 9862.320 us, Deactivation time : 0.161 us)), Write time : 2.234 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409011.311326467] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.104770 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.311267836] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.314854471] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.315962932] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.316025521] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.316364194] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.326678598] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.328770634] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.329087426] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.329165674] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.332007584] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.333449334] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.336709729] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.337830128] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.337870545] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.338996394] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.356977304] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.357344462] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.359973292] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.362518594] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.363474275] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.366577315] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.367953771] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.367985101] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.368252769] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.385557587] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.385984272] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.390264464] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.391449962] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.394649252] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.396071629] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.396112647] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.396503649] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.411317257] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.425152336] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425514362] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425725223] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425781730] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.425809663] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.428261043] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.436395799] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409011.437580595] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409011.440882559] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 32589] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409011.788984488] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409012.208551170] [controller_manager]: Overrun might occur, Total time : 3249.815 us (Expected < 2000.000 us) --> Read time : 2968.210 us, Update time : 278.930 us, Write time : 2.675 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409013.066150608] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.071854915] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.633579 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 32587] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409013.424310066] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409013.424372264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.543200365] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.543282021] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.543451092] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409014.545886393] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.664946 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.564706333] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566258499] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566402332] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566437689] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.566453890] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.568179028] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.574716419] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.574742418] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.576399747] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.592742355] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.593038142] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.593972316] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.596952846] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.596975629] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.597811373] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.614667065] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.615405543] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.616021543] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.618713037] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.618735600] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.619651916] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.634685001] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.635574436] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.636260619] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.639338713] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.639358380] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.639811862] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.653444801] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.654268006] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.667567853] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.667605865] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.667918980] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.683011728] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.684173345] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.691723277] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.691764375] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.692469544] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.708776554] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.710233772] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.713331654] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.713365809] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.713633584] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_2mzucifa -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409014.729014625] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409014.729872685] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 32590] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409016.546436975] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.215869 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409017.669720656] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.499179 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409019.054380047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.158400 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.348171367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.950251 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409020.937156000] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409020.937210133] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.379388443] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.167117 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409021.936779045] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409021.936830403] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409022.936727011] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409022.936786224] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409023.936929120] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409023.936983644] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409024.936780949] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409024.936831305] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409025.398858931] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409025.937238058] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409025.937301318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409025.949971093] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409026.518345970] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409026.518418778] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409026.936841666] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409026.936884748] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409027.936869374] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409027.936929287] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409028.936981644] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409028.937193731] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409029.936771943] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409029.936830324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409030.936850265] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409030.936925698] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409031.936824906] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409031.936883608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409032.936864678] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409032.936918370] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409033.936777707] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409033.936829736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409034.936901721] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409034.936955754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409035.936853648] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409035.936907590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409036.936811406] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409036.936864317] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409013.315860179] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409037.213346839] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:19 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:36751 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:36751 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:20] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.245905266] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.245968446] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.245982232] [moveit_2882792308.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.252387979] [moveit_2882792308.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.254749016] [moveit_2882792308.moveit.ros.rdf_loader]: Loaded robot model in 0.00209667 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.254777200] [moveit_2882792308.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.254784704] [moveit_2882792308.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409023.266574569] [moveit_2882792308.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.269300339] [moveit_2882792308.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.325565849] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.325708581] [moveit_2882792308.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.326718009] [moveit_2882792308.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327277251] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327291939] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327398286] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327900061] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.327988309] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.328585322] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.328598828] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.329053752] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.329505531] [moveit_2882792308.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409023.329734806] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409023.329752169] [moveit_2882792308.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.488595075] [moveit_2882792308.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.489266010] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490406918] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490419261] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490752514] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490766451] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490790015] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490795185] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.490810935] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.491639349] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.493935141] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.493950330] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.496156492] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.496170008] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.496809443] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.526223248] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.530075442] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.530245956] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.530271745] [moveit_2882792308.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409023.531111836] [moveit_2882792308.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:25] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:25] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:26 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547103323] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547236236] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547248579] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547267606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409026.547376312] [moveit_2882792308.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547832829] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409026.547954100] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409036.548437353] [moveit_2882792308.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409036.548540139] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:37] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409037.342777967] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:52 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:52 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:52 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409052.485310461] [moveit_2882792308.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:12 - src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_overhead_obstacle_cylinder.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py::test_overhead_cylinder[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed -======================== 1 failed, 1 warning in 45.79s ========================= - - -12:31:26.36 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests -12:31:26.36 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-979Owj -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py::test_pick_up_mesh_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:31:07 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-54-148841-DESKTOP-HG3Q5KV-1338 (testutils.py:33) - -2026-05-10 12:31:07 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [1343] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [1346] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [1347] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-4]: process started with pid [1348] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-5]: process started with pid [1349] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [robot_state_publisher-2] [INFO] [1778409054.492759738] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.495224431] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.498820369] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.501125339] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.501146269] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.501150327] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409054.501249686] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.667833601] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.671788334] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.672213121] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.672298062] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.681897383] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683529694] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683571223] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683603124] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683609676] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683690940] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409054.683753780] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409054.807033336] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.059384642] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.059443164] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.062480886] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.087157785] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088540412] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088769437] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088799965] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.088826756] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.094806380] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.103418863] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.103471924] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.104823202] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.119556156] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.119886329] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.120792350] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.122915835] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.122949599] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.123662794] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.137034362] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.137388285] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.138036706] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.141342192] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.141380465] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.142238792] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.157029270] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.157362705] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.158371135] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.161467348] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.161533824] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.166797912] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.183353894] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.183754901] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.197198550] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.197250980] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.197982740] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409055.219639942] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.053708 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.223796851] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.224976583] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.237752654] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.237776309] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.238293887] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.274598431] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.275604468] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.290343208] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.290543589] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.292535022] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-5] [INFO] [1778409055.318920026] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409055.328564897] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409055.467057560] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 1349] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409056.326183637] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.597142 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.720130567] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.720178959] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.721608926] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.736888114] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.737940624] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.738021437] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.743496477] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.743962607] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.746842989] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.748051155] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.748086953] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.748475121] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.762982196] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.763290462] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.764335137] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409056.774915572] [controller_manager]: Overrun might occur, Total time : 9221.967 us (Expected < 2000.000 us) --> Read time : 13.325 us, Update time : 9205.196 us (Switch time : 9148.860 us (Switch chained mode time : 0.671 us, perform mode change time : 12.794 us, Activation time : 9118.683 us, Deactivation time : 0.171 us)), Write time : 3.446 us (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.774923050] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.776705768] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.777821703] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.777858092] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.778247301] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.791045094] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.792700765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.793025366] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.793085401] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.796531434] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.797815334] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.800942480] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.802219622] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.802256011] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.803323420] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.820895143] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.821249606] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.823539132] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.826564470] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.827807072] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.830960053] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.832204317] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.832241097] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.832659933] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.851264710] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.851652811] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.854616910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.855810441] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.858838459] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.859963336] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.859998293] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.860455582] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.874767868] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.889223255] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889596389] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889741765] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889753567] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.889762094] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.891435978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.898905166] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409056.900028285] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [spawner-4] [INFO] [1778409056.903250598] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [robot_state_publisher-2] [INFO] [1778409056.925659248] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1346] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 1348] (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.250394589] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409057.250447649] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.162232778] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.646283 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.358299668] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.713674 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.214275760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.689455 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409063.777631938] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.777687103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409064.777955253] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409064.778007863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.777931584] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.777976690] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.112370298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.784094 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [INFO] [1778409066.778073384] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.778151713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [1778409057.113520576] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] [INFO] [1778409059.826449092] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:31:07 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:02 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] * Running on http://127.0.0.1:47197 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] * Running on http://172.30.43.138:47197 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:02] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.153084699] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.153117050] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.153124685] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.163130246] [moveit_1908157456.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.169450425] [moveit_1908157456.moveit.ros.rdf_loader]: Loaded robot model in 0.006204 seconds (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.169484340] [moveit_1908157456.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.169498136] [moveit_1908157456.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [WARN] [1778409066.188733316] [moveit_1908157456.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.192459536] [moveit_1908157456.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.302376041] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.302489857] [moveit_1908157456.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.310336176] [moveit_1908157456.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.310936526] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.310953959] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.311390459] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.311673897] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.311736586] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.312857286] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.312870090] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.313256515] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.313640334] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [WARN] [1778409066.313909546] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [ERROR] [1778409066.313931187] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.693934398] [moveit_1908157456.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.694821654] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696113855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696132530] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696366926] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696379670] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696398656] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696403275] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.696417702] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.697249433] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.699675983] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.699691553] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.701725207] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.701739113] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.702581734] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.731130070] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.735219250] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.735407838] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.735430381] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409066.736174906] [moveit_1908157456.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:07] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:07] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409067.218353106] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:22 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:22 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] 2026-05-10 12:31:22 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:31:25 [ ERROR] [INFO] [1778409082.335691810] [moveit_1908157456.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -________________________ test_pick_up_mesh_by_id[ur5e] _________________________ - -start_processes = Urls(ros_domain_id='6', robot_url='http://0.0.0.0:47197', storage_url='http://127.0.0.1:49373') - - @pytest.mark.timeout(321) - def test_pick_up_mesh_by_id(start_processes: Urls) -> None: - assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" -  - storage_client.URL = start_processes.storage_url -  - storage_client.create_asset( - MESH_ASSET_ID, - MESH_PATH.read_bytes(), - description="Test mesh for UR5e collision avoidance.", - ) -  - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Mesh("Mesh1", MESH_ASSET_ID) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:42: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Mesh1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-54-148841-DESKTOP-HG3Q5KV-1338 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [1343] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [1346] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [1347] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [1348] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [1349] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409054.492759738] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.495224431] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.498820369] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.501125339] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.501146269] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.501150327] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409054.501249686] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.667833601] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.671788334] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.672213121] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.672298062] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.681897383] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683529694] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683571223] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683603124] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683609676] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683690940] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409054.683753780] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409054.807033336] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.059384642] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.059443164] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.062480886] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.087157785] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088540412] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088769437] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088799965] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.088826756] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.094806380] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.103418863] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.103471924] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.104823202] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.119556156] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.119886329] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.120792350] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.122915835] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.122949599] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.123662794] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.137034362] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.137388285] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.138036706] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.141342192] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.141380465] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.142238792] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.157029270] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.157362705] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.158371135] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.161467348] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.161533824] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.166797912] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.183353894] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.183754901] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.197198550] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.197250980] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.197982740] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409055.219639942] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.053708 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.223796851] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.224976583] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.237752654] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.237776309] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.238293887] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.274598431] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.275604468] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.290343208] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.290543589] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.292535022] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409055.318920026] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409055.328564897] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409055.467057560] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 1349] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409056.326183637] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.597142 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.720130567] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.720178959] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.721608926] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.736888114] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.737940624] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.738021437] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.743496477] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.743962607] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.746842989] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.748051155] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.748086953] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.748475121] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.762982196] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.763290462] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.764335137] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409056.774915572] [controller_manager]: Overrun might occur, Total time : 9221.967 us (Expected < 2000.000 us) --> Read time : 13.325 us, Update time : 9205.196 us (Switch time : 9148.860 us (Switch chained mode time : 0.671 us, perform mode change time : 12.794 us, Activation time : 9118.683 us, Deactivation time : 0.171 us)), Write time : 3.446 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.774923050] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.776705768] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.777821703] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.777858092] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.778247301] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.791045094] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.792700765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.793025366] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.793085401] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.796531434] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.797815334] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.800942480] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.802219622] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.802256011] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.803323420] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.820895143] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.821249606] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.823539132] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.826564470] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.827807072] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.830960053] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.832204317] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.832241097] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.832659933] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.851264710] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.851652811] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.854616910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.855810441] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.858838459] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.859963336] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.859998293] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.860455582] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_w5iuudtd -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.874767868] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.889223255] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889596389] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889741765] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889753567] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.889762094] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.891435978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.898905166] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409056.900028285] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409056.903250598] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409056.925659248] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1346] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 1348] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.250394589] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409057.250447649] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.162232778] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.646283 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.358299668] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.713674 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.214275760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.689455 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409063.777631938] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.777687103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409064.777955253] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409064.778007863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.777931584] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.777976690] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.112370298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.784094 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409066.778073384] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.778151713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409057.113520576] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409059.826449092] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:47197 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:47197 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:02] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.153084699] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.153117050] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.153124685] [moveit_1908157456.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.163130246] [moveit_1908157456.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.169450425] [moveit_1908157456.moveit.ros.rdf_loader]: Loaded robot model in 0.006204 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.169484340] [moveit_1908157456.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.169498136] [moveit_1908157456.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409066.188733316] [moveit_1908157456.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.192459536] [moveit_1908157456.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.302376041] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.302489857] [moveit_1908157456.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.310336176] [moveit_1908157456.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.310936526] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.310953959] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.311390459] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.311673897] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.311736586] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.312857286] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.312870090] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.313256515] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.313640334] [moveit_1908157456.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409066.313909546] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409066.313931187] [moveit_1908157456.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.693934398] [moveit_1908157456.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.694821654] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696113855] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696132530] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696366926] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696379670] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696398656] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696403275] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.696417702] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.697249433] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.699675983] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.699691553] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.701725207] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.701739113] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.702581734] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.731130070] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.735219250] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.735407838] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.735430381] [moveit_1908157456.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409066.736174906] [moveit_1908157456.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:07] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:07] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409067.218353106] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409082.335691810] [moveit_1908157456.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:18 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_mesh_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py::test_pick_up_mesh_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 32.42s ========================= - - -12:32:05.12 [INFO] Long running tasks: - 69.28s Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests - 76.08s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests - 85.54s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:32:06.34 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests -12:32:06.34 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-8F8uhs -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py::test_side_mesh[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:31:47 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-57-056550-DESKTOP-HG3Q5KV-1587 (testutils.py:33) - -2026-05-10 12:31:47 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [1625] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [1626] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [1627] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-4]: process started with pid [1628] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-5]: process started with pid [1629] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [robot_state_publisher-2] [INFO] [1778409057.393905761] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.398271588] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.401531231] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.403677229] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.403700123] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.403705473] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409057.403846946] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.573432623] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.576825971] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.577266658] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.577349074] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.586116609] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587551216] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587585390] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587617542] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587624284] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587703435] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.587760102] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409057.710115326] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.962415467] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.962489227] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.964955464] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409057.977448988] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.978520489] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.978641539] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.985778886] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.986405105] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409057.989159238] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.989878820] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.989907725] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409057.994642557] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.007232985] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.007513208] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.008885305] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409058.020060278] [controller_manager]: Overrun might occur, Total time : 9884.320 us (Expected < 2000.000 us) --> Read time : 20.448 us, Update time : 9861.057 us (Switch time : 9804.910 us (Switch chained mode time : 0.701 us, perform mode change time : 13.325 us, Activation time : 9772.639 us, Deactivation time : 0.241 us)), Write time : 2.815 us (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409058.020116365] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.034311 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.020066526] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.023476199] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.024476024] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.024539094] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.024963952] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.036027428] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.037506413] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.037764519] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.037820715] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.041107877] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.042292683] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.045288335] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.046203118] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.046239548] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.047357061] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.061734690] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.062020970] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.064305706] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.067072413] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.068302605] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.071378369] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.072343950] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.072363026] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.072693589] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.085240775] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.085493820] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.088854938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.090351356] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.093592744] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.094845620] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.094905764] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.095256531] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.106564243] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.119186711] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119467239] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119672369] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119725149] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.119755817] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.122436821] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.129093284] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.130423642] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-4] [INFO] [1778409058.133748255] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.255986147] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 1628] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.508711119] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.508770312] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.509235616] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.533349936] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534511864] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534640008] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534668832] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.534683070] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.535924523] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.541650262] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.541688064] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.543106248] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.555128067] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.555394016] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.556107242] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.559578909] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.559614877] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.560297519] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.575131896] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.575369983] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.575958231] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.577621296] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.577655401] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.578396840] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.592978542] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.593216900] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.593747218] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.595627605] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.595663383] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.595948074] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.608997735] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.609283259] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.617497431] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.617550161] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.617781932] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.631246410] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.631508082] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.637922836] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.637990415] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.638375422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.653243147] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.653496293] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.657699070] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.657737984] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.658099762] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [spawner-5] [INFO] [1778409058.673327731] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409058.673608484] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 1629] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.158212859] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.130965 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [robot_state_publisher-2] [INFO] [1778409059.826465709] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1626] (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.014038409] [controller_manager]: Overrun might occur, Total time : 7879.611 us (Expected < 2000.000 us) --> Read time : 16.792 us, Update time : 7857.720 us, Write time : 5.099 us (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409060.146723717] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.146776758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.358203966] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.122173 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.210785563] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.703469 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.112919441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.837598 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409066.636066602] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.636124843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409067.636477549] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409067.636526131] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409068.636422189] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409068.636468367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409069.414015023] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.932839 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409069.636516783] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409069.636569804] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409070.636505064] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409070.636560870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409071.397596444] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409071.636440927] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409071.636492324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409071.948646189] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409072.526736679] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.526791333] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.636558699] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409072.636609195] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.723430772] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.723510634] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409073.636365181] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409073.636420225] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409074.636178275] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409074.636241675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409075.636207945] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409075.636253812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409076.446235885] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409076.636207022] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409076.636246187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409077.636244745] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409077.636286685] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409078.636233287] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409078.636275667] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409079.636409575] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409079.636466954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409080.636140945] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409080.636191050] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409081.636366728] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409081.636428636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409082.636261368] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409082.636313246] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409083.636647711] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409083.636702325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409084.636333148] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409084.636369046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409085.636258635] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409085.636298361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.382266034] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.183810 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409086.636615062] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.636644398] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409087.636235435] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409087.636641607] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409088.636348004] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409088.636377460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409088.734197088] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.115265 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409089.636389653] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409089.636446601] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409089.977361987] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.386729955] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.647771 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.528256021] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409090.636392147] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.636450007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409091.104154587] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.104229640] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.145009856] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.145070281] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.636299305] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409091.636351965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409092.258234201] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409092.636510541] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.636565316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.690195175] [controller_manager]: Overrun might occur, Total time : 2050.141 us (Expected < 2000.000 us) --> Read time : 16.532 us, Update time : 2031.876 us, Write time : 1.733 us (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.690240521] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.158718 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409093.636391211] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.636449461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.898545401] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.463748 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409094.636317763] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409094.636370543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409095.636504934] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409095.636555720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.619007763] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.926000 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409096.638469374] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.638499200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409097.557072511] [controller_manager]: Overrun might occur, Total time : 2919.579 us (Expected < 2000.000 us) --> Read time : 14.267 us, Update time : 2903.739 us, Write time : 1.573 us (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409097.636409950] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409097.636462670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409098.636442343] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409098.636492508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409099.636344232] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409099.636407863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409099.961066696] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.984482 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409100.636464336] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409100.636520132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409101.410284316] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.202303 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409101.636283906] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409101.636325245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409102.636625025] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409102.636670882] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409103.636342079] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.636403837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.731012425] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930311 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.747636241] [controller_manager]: Overrun might occur, Total time : 2598.958 us (Expected < 2000.000 us) --> Read time : 25.048 us, Update time : 2571.415 us, Write time : 2.495 us (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409104.636463729] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409104.636516870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409105.636609049] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409105.636657461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [INFO] [1778409106.636902064] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [ros2_control_node-1] [WARN] [1778409106.636959924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [1778409060.030988337] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] [INFO] [1778409090.383598995] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:31:47 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] * Running on http://127.0.0.1:56347 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] * Running on http://172.30.43.138:56347 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:05] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.249470348] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.249524581] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.249541453] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.256581019] [moveit_3227491728.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.258494535] [moveit_3227491728.moveit.ros.rdf_loader]: Loaded robot model in 0.00175904 seconds (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.258521055] [moveit_3227491728.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.258531044] [moveit_3227491728.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [WARN] [1778409069.269987853] [moveit_3227491728.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.272974759] [moveit_3227491728.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.325908069] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.326026614] [moveit_3227491728.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.326990465] [moveit_3227491728.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327486678] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327501637] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327611415] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.327970518] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.328041683] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.328869781] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.328886433] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.329438131] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.329938833] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [WARN] [1778409069.330222959] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [ERROR] [1778409069.330247545] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.399311243] [moveit_3227491728.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.400248770] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401510396] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401525936] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401832624] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401846460] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401870175] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401875746] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.401886727] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.402776557] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.405202514] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.405216600] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.407487981] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.407502739] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.408249692] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.782849453] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.786708094] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.786883868] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.786913144] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409069.787926139] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:11] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:11] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684143616] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684298691] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684330622] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.684374645] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [WARN] [1778409072.684528999] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.685146251] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.685258144] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.717025656] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.719363518] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.719937599] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:12 INFO  Executing plan (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720709100] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720754927] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720792619] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720842614] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.720867050] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722670537] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722702167] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722745710] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.722882450] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.723764370] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409072.723784989] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409076.474738864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409076.480470850] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:16] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:17] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:17] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:20] "PUT /collisions/mesh?meshId=Mesh2&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:20] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:23] "PUT /collisions/mesh?meshId=Mesh3&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:23] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:26] "PUT /collisions/mesh?meshId=Mesh4&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:26] "GET /collisions HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115116240] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115183888] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115194699] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115205920] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [WARN] [1778409091.115263620] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115541498] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.115585311] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.141696641] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.141989909] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142309786] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Executing plan (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142632400] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142656085] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142675962] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142684829] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.142705388] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144522625] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144556199] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144578572] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.144696235] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.145302849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409091.145315924] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409092.295947666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409092.300474879] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:31:47 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self._wait_for_pose_reached(target_abs, tolerance=0.01) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] raise UrGeneral("Target pose was not reached.") (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] arcor2_ur.exceptions.UrGeneral: Target pose was not reached. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:47] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=true HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409107.521062780] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:32:02 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:32:02 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] 2026-05-10 12:32:02 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:32:05 [ ERROR] [INFO] [1778409122.678009905] [moveit_3227491728.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________________ test_side_mesh[ur5e] _____________________________ - -start_processes = Urls(ros_domain_id='88', robot_url='http://0.0.0.0:56347', storage_url='http://127.0.0.1:52313') - - @pytest.mark.timeout(400) - def test_side_mesh(start_processes: Urls) -> None: - assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" -  - storage_client.URL = start_processes.storage_url -  - storage_client.create_asset( - MESH_ASSET_ID, - MESH_PATH.read_bytes(), - description="Test mesh for UR5e collision avoidance.", - ) -  - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -  - ot.move_to_pose("", start_pose, 0.3, safe=False) - time.sleep(1) -  - mesh_positions = [ - # rgith - Position(0.5, 0.5, 0.25), - Position(0.5, 0.5, 0.55), - # left - Position(-0.5, 0.5, 0.25), - Position(-0.5, 0.5, 0.55), - ] -  - for idx, position in enumerate(mesh_positions, start=1): - mesh = Mesh(f"Mesh{idx}", MESH_ASSET_ID) -  - scene_service.upsert_collision( - mesh, - Pose(position, Orientation(0, 0, 0, 1)), - ) -  - assert mesh.id in scene_service.collision_ids() - time.sleep(3) -  -> ot.move_to_pose("", goal_pose, 0.3) - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:66: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not reached. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-57-056550-DESKTOP-HG3Q5KV-1587 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [1625] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [1626] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [1627] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [1628] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [1629] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409057.393905761] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.398271588] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.401531231] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.403677229] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.403700123] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.403705473] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409057.403846946] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.573432623] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.576825971] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.577266658] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.577349074] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.586116609] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587551216] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587585390] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587617542] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587624284] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587703435] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.587760102] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409057.710115326] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.962415467] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.962489227] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.964955464] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409057.977448988] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.978520489] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.978641539] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.985778886] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.986405105] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409057.989159238] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.989878820] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.989907725] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409057.994642557] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.007232985] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.007513208] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.008885305] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409058.020060278] [controller_manager]: Overrun might occur, Total time : 9884.320 us (Expected < 2000.000 us) --> Read time : 20.448 us, Update time : 9861.057 us (Switch time : 9804.910 us (Switch chained mode time : 0.701 us, perform mode change time : 13.325 us, Activation time : 9772.639 us, Deactivation time : 0.241 us)), Write time : 2.815 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409058.020116365] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.034311 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.020066526] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.023476199] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.024476024] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.024539094] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.024963952] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.036027428] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.037506413] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.037764519] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.037820715] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.041107877] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.042292683] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.045288335] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.046203118] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.046239548] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.047357061] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.061734690] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.062020970] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.064305706] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.067072413] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.068302605] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.071378369] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.072343950] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.072363026] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.072693589] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.085240775] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.085493820] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.088854938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.090351356] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.093592744] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.094845620] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.094905764] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.095256531] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.106564243] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.119186711] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119467239] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119672369] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119725149] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.119755817] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.122436821] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.129093284] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.130423642] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409058.133748255] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.255986147] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 1628] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.508711119] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.508770312] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.509235616] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.533349936] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534511864] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534640008] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534668832] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.534683070] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.535924523] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.541650262] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.541688064] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.543106248] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.555128067] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.555394016] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.556107242] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.559578909] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.559614877] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.560297519] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.575131896] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.575369983] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.575958231] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.577621296] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.577655401] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.578396840] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.592978542] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.593216900] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.593747218] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.595627605] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.595663383] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.595948074] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.608997735] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.609283259] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.617497431] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.617550161] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.617781932] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.631246410] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.631508082] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.637922836] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.637990415] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.638375422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.653243147] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.653496293] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.657699070] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.657737984] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.658099762] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_nfxcwrj7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409058.673327731] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409058.673608484] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 1629] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.158212859] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.130965 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409059.826465709] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1626] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.014038409] [controller_manager]: Overrun might occur, Total time : 7879.611 us (Expected < 2000.000 us) --> Read time : 16.792 us, Update time : 7857.720 us, Write time : 5.099 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409060.146723717] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.146776758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.358203966] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.122173 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.210785563] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.703469 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.112919441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.837598 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409066.636066602] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.636124843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409067.636477549] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409067.636526131] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409068.636422189] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409068.636468367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409069.414015023] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.932839 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409069.636516783] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409069.636569804] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409070.636505064] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409070.636560870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409071.397596444] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409071.636440927] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409071.636492324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409071.948646189] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409072.526736679] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.526791333] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.636558699] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409072.636609195] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.723430772] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.723510634] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409073.636365181] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409073.636420225] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409074.636178275] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409074.636241675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409075.636207945] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409075.636253812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409076.446235885] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409076.636207022] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409076.636246187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409077.636244745] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409077.636286685] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409078.636233287] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409078.636275667] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409079.636409575] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409079.636466954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409080.636140945] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409080.636191050] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409081.636366728] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409081.636428636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409082.636261368] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409082.636313246] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409083.636647711] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409083.636702325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409084.636333148] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409084.636369046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409085.636258635] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409085.636298361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.382266034] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.183810 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409086.636615062] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.636644398] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409087.636235435] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409087.636641607] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409088.636348004] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409088.636377460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409088.734197088] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.115265 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409089.636389653] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409089.636446601] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409089.977361987] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.386729955] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.647771 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.528256021] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409090.636392147] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.636450007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409091.104154587] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.104229640] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.145009856] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.145070281] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.636299305] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409091.636351965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409092.258234201] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409092.636510541] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.636565316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.690195175] [controller_manager]: Overrun might occur, Total time : 2050.141 us (Expected < 2000.000 us) --> Read time : 16.532 us, Update time : 2031.876 us, Write time : 1.733 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.690240521] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.158718 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409093.636391211] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.636449461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.898545401] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.463748 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409094.636317763] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409094.636370543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409095.636504934] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409095.636555720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.619007763] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.926000 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409096.638469374] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.638499200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409097.557072511] [controller_manager]: Overrun might occur, Total time : 2919.579 us (Expected < 2000.000 us) --> Read time : 14.267 us, Update time : 2903.739 us, Write time : 1.573 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409097.636409950] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409097.636462670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409098.636442343] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409098.636492508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409099.636344232] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409099.636407863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409099.961066696] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.984482 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409100.636464336] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409100.636520132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409101.410284316] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.202303 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409101.636283906] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409101.636325245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409102.636625025] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409102.636670882] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409103.636342079] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.636403837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.731012425] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.930311 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.747636241] [controller_manager]: Overrun might occur, Total time : 2598.958 us (Expected < 2000.000 us) --> Read time : 25.048 us, Update time : 2571.415 us, Write time : 2.495 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409104.636463729] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409104.636516870] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409105.636609049] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409105.636657461] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409106.636902064] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409106.636959924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409060.030988337] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409090.383598995] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:56347 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:56347 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:05] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.249470348] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.249524581] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.249541453] [moveit_3227491728.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.256581019] [moveit_3227491728.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.258494535] [moveit_3227491728.moveit.ros.rdf_loader]: Loaded robot model in 0.00175904 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.258521055] [moveit_3227491728.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.258531044] [moveit_3227491728.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409069.269987853] [moveit_3227491728.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.272974759] [moveit_3227491728.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.325908069] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.326026614] [moveit_3227491728.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.326990465] [moveit_3227491728.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327486678] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327501637] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327611415] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.327970518] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.328041683] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.328869781] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.328886433] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.329438131] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.329938833] [moveit_3227491728.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409069.330222959] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409069.330247545] [moveit_3227491728.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.399311243] [moveit_3227491728.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.400248770] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401510396] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401525936] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401832624] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401846460] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401870175] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401875746] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.401886727] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.402776557] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.405202514] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.405216600] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.407487981] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.407502739] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.408249692] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.782849453] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.786708094] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.786883868] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.786913144] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409069.787926139] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:11] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:11] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684143616] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684298691] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684330622] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.684374645] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409072.684528999] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.685146251] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.685258144] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.717025656] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.719363518] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.719937599] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720709100] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720754927] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720792619] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720842614] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.720867050] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722670537] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722702167] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722745710] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.722882450] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.723764370] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409072.723784989] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409076.474738864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409076.480470850] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:16] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:17] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:17] "GET /collisions HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:20] "PUT /collisions/mesh?meshId=Mesh2&meshFileId=triangle_block.stl HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:20] "GET /collisions HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:23] "PUT /collisions/mesh?meshId=Mesh3&meshFileId=triangle_block.stl HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:23] "GET /collisions HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:26] "PUT /collisions/mesh?meshId=Mesh4&meshFileId=triangle_block.stl HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:26] "GET /collisions HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115116240] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115183888] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115194699] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115205920] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409091.115263620] [moveit_3227491728.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115541498] [moveit_3227491728.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.115585311] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.141696641] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.141989909] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142309786] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142632400] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142656085] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142675962] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142684829] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.142705388] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144522625] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144556199] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144578572] [moveit_3227491728.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.144696235] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.145302849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409091.145315924] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409092.295947666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409092.300474879] [moveit_3227491728.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:47 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 738, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._wait_for_pose_reached(target_abs, tolerance=0.01) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 688, in _wait_for_pose_reached -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Target pose was not reached.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Target pose was not reached. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:47] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=true HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409107.521062780] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:02 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:02 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:02 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409122.678009905] [moveit_3227491728.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:17 - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:17: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_mesh.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py::test_side_mesh[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Target pose was not ... -=================== 1 failed, 1 warning in 69.45s (0:01:09) ==================== - - -12:32:11.98 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests -12:32:11.99 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-Lj2y79 -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py::test_pick_and_place_mesh_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:31:52 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-50-208901-DESKTOP-HG3Q5KV-1093 (testutils.py:33) - -2026-05-10 12:31:52 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [1108] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [1109] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [1110] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-4]: process started with pid [1111] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-5]: process started with pid [1112] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409050.534796294] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.537717464] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.539797397] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.541434227] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.541465627] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.541472660] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409050.541572230] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409050.826096610] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.881925906] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.884237448] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.884584057] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.884652186] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.892684554] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893460167] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893475166] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893495414] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893499502] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893554296] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409050.893585987] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.078451158] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.078511332] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.081136871] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.103279230] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.104810183] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.105073624] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.105125582] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.105155148] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.110051927] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.117058706] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.117100085] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.117948066] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.131007942] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.131281597] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.132131061] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.135425550] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.135471017] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.136281983] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.149171840] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.149438878] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.150040421] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.153769461] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.153841478] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.154597345] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.169772434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.170196635] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.170745598] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.173786576] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.173830579] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.177773808] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.190955349] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.191269275] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.201557575] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.201600797] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.201947716] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.219079168] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.219347022] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.225294273] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.225352233] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.225690160] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.239257684] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.239552910] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.243234370] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.243277932] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.243831860] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-5] [INFO] [1778409051.259697979] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.260074338] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.396575070] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 1112] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.649298425] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.649350063] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.650571073] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.667248592] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.668532653] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.668628866] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.673671282] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.674127448] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.677083259] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.678549010] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.678591761] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.679071757] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.695225944] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.695591267] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.696701116] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409051.709291935] [controller_manager]: Overrun might occur, Total time : 11376.546 us (Expected < 2000.000 us) --> Read time : 13.235 us, Update time : 11360.065 us (Switch time : 11312.333 us (Switch chained mode time : 0.421 us, perform mode change time : 14.718 us, Activation time : 11280.353 us, Deactivation time : 0.180 us)), Write time : 3.246 us (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409051.709347380] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.520978 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.709308296] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.710980593] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.712138253] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.712199029] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.712583239] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.727533787] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.729140805] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.729507187] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.729569315] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.732760378] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.734021689] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.736963655] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.738046052] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.738084866] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.739318594] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.759246234] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.759640443] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.761893724] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.764864128] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.766030391] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.768986255] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.770436371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.770476868] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.770956344] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.789394509] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.789769527] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.792833303] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.794043797] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.797385648] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.798823245] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.798891695] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.799318336] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.814566600] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.829250391] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.829801739] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.830022047] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.830045101] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.830059328] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.831885137] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.841167424] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409051.842297711] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [spawner-4] [INFO] [1778409051.845047848] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 1111] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409052.940765860] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1109] (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409053.125566390] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.740318 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409053.331487855] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409053.331539724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409056.062209410] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.383449 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.162219974] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.393683 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409059.762061294] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409059.762094558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.358231412] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.405491 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409060.673485740] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409060.673540484] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409061.673384822] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409061.673439777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409062.062225156] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.399055 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409062.673376527] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409062.673427474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.214282604] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.456503 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409063.673473817] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409063.673525916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409064.467116066] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409064.673560281] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409064.673611989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.018151099] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.673554340] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.673606920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409065.708278545] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.708316918] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.800995232] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409065.801050257] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.118272026] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.446015 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.148850314] [controller_manager]: Overrun might occur, Total time : 4929.837 us (Expected < 2000.000 us) --> Read time : 330.638 us, Update time : 4597.316 us, Write time : 1.883 us (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409066.673536295] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409066.673608743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409067.315934628] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409067.673511368] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409067.673559750] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409068.673245813] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409068.673305376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409069.673284334] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409069.673333839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409070.673452288] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409070.673525267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409071.673328758] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409071.673378513] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409072.673503119] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409072.673556400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409073.673474053] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409073.673525290] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409074.673346934] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409074.673390227] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409075.673389485] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409075.673437286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409076.673370617] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409076.673413779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409077.673247479] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409077.673294198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409078.673365653] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409078.673416199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409079.673473622] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409079.673527384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409080.673241165] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409080.673287714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409081.673360887] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409081.673410240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409082.673613631] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409082.673665219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409083.673528594] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409083.673583809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409084.673610609] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409084.673659832] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409085.673233739] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409085.673281590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.620302547] [controller_manager]: Overrun might occur, Total time : 2454.604 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2437.962 us, Write time : 1.523 us (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.620345719] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.519497 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409086.674855954] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409086.674890309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409087.673349401] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409087.673396140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409088.673695528] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409088.673753819] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409089.673320681] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409089.673367941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409090.673293690] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409090.673342302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409091.673300202] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409091.673350608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.660275078] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.448827 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409092.675720485] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409092.675749481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409093.673633496] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.673688570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.845895244] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.068972 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409093.959743805] [controller_manager]: Overrun might occur, Total time : 3776.131 us (Expected < 2000.000 us) --> Read time : 14.688 us, Update time : 3759.659 us, Write time : 1.784 us (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409094.673440575] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409094.673487163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409095.673717343] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409095.673778779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.619157513] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.331612 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.624662428] [controller_manager]: Overrun might occur, Total time : 4756.027 us (Expected < 2000.000 us) --> Read time : 15.330 us, Update time : 4738.834 us, Write time : 1.863 us (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409096.673973175] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409096.674008051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409097.673698728] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409097.673753683] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409098.673411690] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409098.673470191] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409099.673576313] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409099.673646887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409100.673355515] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409100.673404558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409101.673561053] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409101.673613252] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409102.673568321] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409102.673615831] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409103.673498821] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.673552864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.749625385] [controller_manager]: Overrun might occur, Total time : 4545.581 us (Expected < 2000.000 us) --> Read time : 19.046 us, Update time : 4523.880 us, Write time : 2.655 us (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409103.754227866] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.400994 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409104.673332985] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409104.673379834] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409105.673497858] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409105.673543965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409106.673637250] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409106.673696984] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409107.673716033] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409107.673767992] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409108.673330679] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409108.673375765] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409109.673348939] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409109.673401128] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409110.673397454] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409110.673450174] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409111.673368327] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409111.673421348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409112.673303607] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409112.673368180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [1778409053.205450453] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] [INFO] [1778409056.925656221] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:31:52 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:30:58 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] * Running on http://127.0.0.1:35607 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] * Running on http://172.30.43.138:35607 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:30:58] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:02 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.042274388] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.042334121] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.042348970] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.048385659] [moveit_2243744926.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.050200017] [moveit_2243744926.moveit.ros.rdf_loader]: Loaded robot model in 0.00165471 seconds (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.050239682] [moveit_2243744926.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.050261754] [moveit_2243744926.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [WARN] [1778409063.062221408] [moveit_2243744926.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.065425446] [moveit_2243744926.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.128810330] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.128956397] [moveit_2243744926.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130085512] [moveit_2243744926.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130662589] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130678409] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.130828108] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131274186] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131360430] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131730994] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.131747486] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.132953703] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.133378611] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [WARN] [1778409063.133619439] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [ERROR] [1778409063.133635269] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.203732351] [moveit_2243744926.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.204606873] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206344545] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206362569] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206650987] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206664232] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206687787] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206693207] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.206704439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.207390292] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.209663841] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.209677668] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.212455336] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.212470064] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.213385363] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.244085982] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.258667178] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.258946078] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.258995763] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409063.263513246] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:03] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  Generated 1 grasp options for object Mesh1. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  IK accepted: 1 grasp options. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768116709] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768277775] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768316679] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768355032] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [WARN] [1778409065.768426778] [moveit_2243744926.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.768893816] [moveit_2243744926.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.769003915] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.785176689] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.785656140] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.785925552] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:05 INFO  Executing plan (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798546545] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798572955] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798595648] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798630495] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.798651505] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800235478] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800274182] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800332112] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.800473069] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.801281791] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409065.801313110] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409067.352128756] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409067.358165727] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:22 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:31:52 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 127.0.0.1 - - [10/May/2026 12:31:52] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409112.955506467] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:32:07 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:32:07 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] 2026-05-10 12:32:08 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:32:11 [ ERROR] [INFO] [1778409128.127482900] [moveit_2243744926.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -__________________ test_pick_and_place_mesh_by_position[ur5e] __________________ - -start_processes = Urls(ros_domain_id='24', robot_url='http://0.0.0.0:35607', storage_url='http://127.0.0.1:39361') - - @pytest.mark.timeout(321) - def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None: - assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" -  - storage_client.URL = start_processes.storage_url -  - storage_client.create_asset( - MESH_ASSET_ID, - MESH_PATH.read_bytes(), - description="Test mesh for UR5e collision avoidance.", - ) -  - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Mesh("Mesh1", MESH_ASSET_ID) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:45: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Mesh1. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-30-50-208901-DESKTOP-HG3Q5KV-1093 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [1108] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [1109] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [1110] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [1111] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [1112] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409050.534796294] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.537717464] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.539797397] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.541434227] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.541465627] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.541472660] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409050.541572230] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409050.826096610] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.881925906] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.884237448] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.884584057] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.884652186] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.892684554] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893460167] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893475166] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893495414] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893499502] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893554296] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409050.893585987] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.078451158] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.078511332] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.081136871] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.103279230] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.104810183] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.105073624] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.105125582] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.105155148] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.110051927] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.117058706] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.117100085] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.117948066] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.131007942] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.131281597] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.132131061] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.135425550] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.135471017] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.136281983] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.149171840] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.149438878] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.150040421] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.153769461] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.153841478] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.154597345] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.169772434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.170196635] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.170745598] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.173786576] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.173830579] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.177773808] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.190955349] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.191269275] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.201557575] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.201600797] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.201947716] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.219079168] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.219347022] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.225294273] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.225352233] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.225690160] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.239257684] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.239552910] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.243234370] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.243277932] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.243831860] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409051.259697979] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.260074338] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.396575070] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 1112] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.649298425] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.649350063] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.650571073] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.667248592] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.668532653] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.668628866] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.673671282] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.674127448] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.677083259] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.678549010] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.678591761] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.679071757] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.695225944] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.695591267] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.696701116] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409051.709291935] [controller_manager]: Overrun might occur, Total time : 11376.546 us (Expected < 2000.000 us) --> Read time : 13.235 us, Update time : 11360.065 us (Switch time : 11312.333 us (Switch chained mode time : 0.421 us, perform mode change time : 14.718 us, Activation time : 11280.353 us, Deactivation time : 0.180 us)), Write time : 3.246 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409051.709347380] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.520978 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.709308296] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.710980593] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.712138253] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.712199029] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.712583239] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.727533787] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.729140805] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.729507187] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.729569315] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.732760378] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.734021689] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.736963655] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.738046052] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.738084866] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.739318594] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.759246234] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.759640443] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.761893724] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.764864128] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.766030391] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.768986255] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.770436371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.770476868] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.770956344] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.789394509] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.789769527] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.792833303] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.794043797] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.797385648] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.798823245] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.798891695] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.799318336] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_gsgr_6_f -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.814566600] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.829250391] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.829801739] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.830022047] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.830045101] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.830059328] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.831885137] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.841167424] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409051.842297711] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409051.845047848] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 1111] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409052.940765860] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 1109] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409053.125566390] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.740318 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409053.331487855] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409053.331539724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409056.062209410] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.383449 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.162219974] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.393683 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409059.762061294] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409059.762094558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.358231412] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.405491 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409060.673485740] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409060.673540484] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409061.673384822] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409061.673439777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409062.062225156] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.399055 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409062.673376527] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409062.673427474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.214282604] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.456503 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409063.673473817] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409063.673525916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409064.467116066] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409064.673560281] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409064.673611989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.018151099] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.673554340] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.673606920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409065.708278545] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.708316918] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.800995232] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409065.801050257] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.118272026] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.446015 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.148850314] [controller_manager]: Overrun might occur, Total time : 4929.837 us (Expected < 2000.000 us) --> Read time : 330.638 us, Update time : 4597.316 us, Write time : 1.883 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409066.673536295] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409066.673608743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409067.315934628] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409067.673511368] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409067.673559750] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409068.673245813] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409068.673305376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409069.673284334] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409069.673333839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409070.673452288] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409070.673525267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409071.673328758] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409071.673378513] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409072.673503119] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409072.673556400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409073.673474053] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409073.673525290] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409074.673346934] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409074.673390227] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409075.673389485] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409075.673437286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409076.673370617] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409076.673413779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409077.673247479] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409077.673294198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409078.673365653] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409078.673416199] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409079.673473622] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409079.673527384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409080.673241165] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409080.673287714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409081.673360887] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409081.673410240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409082.673613631] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409082.673665219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409083.673528594] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409083.673583809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409084.673610609] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409084.673659832] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409085.673233739] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409085.673281590] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.620302547] [controller_manager]: Overrun might occur, Total time : 2454.604 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2437.962 us, Write time : 1.523 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.620345719] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.519497 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409086.674855954] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409086.674890309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409087.673349401] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409087.673396140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409088.673695528] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409088.673753819] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409089.673320681] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409089.673367941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409090.673293690] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409090.673342302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409091.673300202] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409091.673350608] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.660275078] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.448827 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409092.675720485] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409092.675749481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409093.673633496] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.673688570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.845895244] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.068972 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409093.959743805] [controller_manager]: Overrun might occur, Total time : 3776.131 us (Expected < 2000.000 us) --> Read time : 14.688 us, Update time : 3759.659 us, Write time : 1.784 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409094.673440575] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409094.673487163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409095.673717343] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409095.673778779] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.619157513] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.331612 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.624662428] [controller_manager]: Overrun might occur, Total time : 4756.027 us (Expected < 2000.000 us) --> Read time : 15.330 us, Update time : 4738.834 us, Write time : 1.863 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409096.673973175] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409096.674008051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409097.673698728] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409097.673753683] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409098.673411690] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409098.673470191] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409099.673576313] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409099.673646887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409100.673355515] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409100.673404558] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409101.673561053] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409101.673613252] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409102.673568321] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409102.673615831] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409103.673498821] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.673552864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.749625385] [controller_manager]: Overrun might occur, Total time : 4545.581 us (Expected < 2000.000 us) --> Read time : 19.046 us, Update time : 4523.880 us, Write time : 2.655 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409103.754227866] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.400994 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409104.673332985] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409104.673379834] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409105.673497858] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409105.673543965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409106.673637250] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409106.673696984] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409107.673716033] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409107.673767992] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409108.673330679] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409108.673375765] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409109.673348939] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409109.673401128] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409110.673397454] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409110.673450174] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409111.673368327] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409111.673421348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409112.673303607] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409112.673368180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409053.205450453] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409056.925656221] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:30:58 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:35607 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:35607 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:30:58] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.042274388] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.042334121] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.042348970] [moveit_2243744926.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.048385659] [moveit_2243744926.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.050200017] [moveit_2243744926.moveit.ros.rdf_loader]: Loaded robot model in 0.00165471 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.050239682] [moveit_2243744926.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.050261754] [moveit_2243744926.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409063.062221408] [moveit_2243744926.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.065425446] [moveit_2243744926.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.128810330] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.128956397] [moveit_2243744926.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130085512] [moveit_2243744926.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130662589] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130678409] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.130828108] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131274186] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131360430] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131730994] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.131747486] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.132953703] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.133378611] [moveit_2243744926.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409063.133619439] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409063.133635269] [moveit_2243744926.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.203732351] [moveit_2243744926.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.204606873] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206344545] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206362569] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206650987] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206664232] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206687787] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206693207] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.206704439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.207390292] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.209663841] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.209677668] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.212455336] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.212470064] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.213385363] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.244085982] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.258667178] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.258946078] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.258995763] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409063.263513246] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:03] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:03] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  Generated 1 grasp options for object Mesh1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  IK accepted: 1 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768116709] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768277775] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768316679] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768355032] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409065.768426778] [moveit_2243744926.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.768893816] [moveit_2243744926.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.769003915] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.785176689] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.785656140] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.785925552] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:05 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798546545] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798572955] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798595648] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798630495] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.798651505] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800235478] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800274182] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800332112] [moveit_2243744926.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.800473069] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.801281791] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409065.801313110] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409067.352128756] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409067.358165727] [moveit_2243744926.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:31:52 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:31:52] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409112.955506467] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:07 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:07 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:08 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409128.127482900] [moveit_2243744926.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:18 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_mesh_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py::test_pick_and_place_mesh_by_position[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... -=================== 1 failed, 1 warning in 82.06s (0:01:22) ==================== - - -12:32:35.18 [INFO] Long running tasks: - 68.82s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 115.60s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:32:38.20 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests -12:32:38.20 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-TEAde1 -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py::test_pick_up_cylinder_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:32:19 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-07-640294-DESKTOP-HG3Q5KV-3052 (testutils.py:33) - -2026-05-10 12:32:19 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [3056] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [3057] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [3058] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-4]: process started with pid [3059] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-5]: process started with pid [3060] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [robot_state_publisher-2] [INFO] [1778409127.993664675] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409127.996944521] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.000574068] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.002839793] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.002858950] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.002864220] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409128.002962276] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.181365313] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.185645682] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.186153106] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.186215374] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.194459603] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196573390] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196611933] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196639596] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196646229] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196725139] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.196799610] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.344241989] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409128.519749112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.503087 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.678025591] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.678093881] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.681192939] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.694936324] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.696114086] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.696314838] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.707767143] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.709626737] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.713051289] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.714370847] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.714435009] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.720916324] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.733456382] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.733849429] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.736309753] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409128.751167685] [controller_manager]: Overrun might occur, Total time : 13823.967 us (Expected < 2000.000 us) --> Read time : 20.018 us, Update time : 13799.000 us (Switch time : 13740.969 us (Switch chained mode time : 0.441 us, perform mode change time : 15.489 us, Activation time : 13706.855 us, Deactivation time : 0.491 us)), Write time : 4.949 us (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.751190203] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.752958608] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.753963963] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.754038745] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.754311483] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.764250954] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.767190920] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.767588205] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.767709786] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.770711971] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.771464371] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.774687324] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.776033723] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.776058350] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.777694970] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.792778190] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.793083204] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.795951489] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.798880154] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.799518321] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.802756815] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.804679343] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.804706885] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.804946751] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.817030312] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.818030457] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.820535522] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.821497990] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.825206983] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.826818400] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.826891930] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.827192852] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.837350557] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.848832494] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849504902] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849781107] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849812507] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.849838216] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.853181318] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.862805651] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409128.863711833] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-4] [INFO] [1778409128.867325184] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409129.005576102] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 3059] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.260059673] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.260113485] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.260787582] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.284964681] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368481357] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368722636] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368764676] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.368781447] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.371023768] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.377663194] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.377726424] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.379264296] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.396826636] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.397933018] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.399216362] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.404014219] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.404059425] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.405038495] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [robot_state_publisher-2] [INFO] [1778409130.409930156] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.425512078] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.426795136] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.427700386] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.431824462] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.431866121] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.432687893] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.449164459] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.449616384] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.450960872] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409130.453804823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.558878 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.456153660] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.456179930] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.456369480] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.471233203] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.473173439] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.484151273] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.484200247] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.484707255] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.502925638] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.503308690] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.511922644] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.511970775] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.512378129] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3057] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.534818142] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.535604206] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.539977866] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.540009266] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.540606385] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [spawner-5] [INFO] [1778409130.559346830] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409130.559769223] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 3060] (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409131.686666050] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409131.686748396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409132.000946839] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.701065 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409133.877767277] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.521623 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.625747192] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.501367 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409137.102871533] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409137.102919725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409138.103064299] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.103116037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.291325368] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.079613 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [INFO] [1778409139.103057385] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.103107089] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.370623757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.377642 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [1778409130.689048478] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] [INFO] [1778409136.042658337] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:32:19 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] * Running on http://127.0.0.1:60489 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] * Running on http://172.30.43.138:60489 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:16] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.366806514] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.366867329] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.366882588] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.374051630] [moveit_2313487515.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.376068894] [moveit_2313487515.moveit.ros.rdf_loader]: Loaded robot model in 0.00183629 seconds (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.376110854] [moveit_2313487515.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.376121013] [moveit_2313487515.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [WARN] [1778409139.388832181] [moveit_2313487515.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.391833093] [moveit_2313487515.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.449507571] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.449650462] [moveit_2313487515.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.450663467] [moveit_2313487515.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451194186] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451209244] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451356975] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451800528] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.451899025] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.452779328] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.452794878] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.453258188] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.453767276] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [WARN] [1778409139.454022841] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [ERROR] [1778409139.454063078] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.490527964] [moveit_2313487515.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.491455156] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.492869248] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.492886631] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493072474] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493085269] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493106850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493111198] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493121217] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.493867605] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.502151326] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.502165202] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.505305559] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.505319525] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.506323733] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.536448119] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.540276398] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.540541912] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.540566309] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.541600113] [moveit_2313487515.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:19] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:19] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409139.767143883] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:34 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:34 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] 2026-05-10 12:32:34 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:32:37 [ ERROR] [INFO] [1778409154.888994241] [moveit_2313487515.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -______________________ test_pick_up_cylinder_by_id[ur5e] _______________________ - -start_processes = Urls(ros_domain_id='223', robot_url='http://0.0.0.0:60489', storage_url='http://127.0.0.1:55293') - - @pytest.mark.timeout(321) - def test_pick_up_cylinder_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Cylinder("Cyl1", 0.1, 0.2) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Cyl1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-07-640294-DESKTOP-HG3Q5KV-3052 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [3056] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [3057] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [3058] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [3059] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [3060] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409127.993664675] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409127.996944521] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.000574068] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.002839793] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.002858950] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.002864220] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409128.002962276] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.181365313] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.185645682] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.186153106] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.186215374] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.194459603] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196573390] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196611933] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196639596] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196646229] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196725139] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.196799610] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.344241989] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409128.519749112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.503087 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.678025591] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.678093881] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.681192939] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.694936324] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.696114086] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.696314838] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.707767143] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.709626737] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.713051289] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.714370847] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.714435009] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.720916324] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.733456382] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.733849429] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.736309753] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409128.751167685] [controller_manager]: Overrun might occur, Total time : 13823.967 us (Expected < 2000.000 us) --> Read time : 20.018 us, Update time : 13799.000 us (Switch time : 13740.969 us (Switch chained mode time : 0.441 us, perform mode change time : 15.489 us, Activation time : 13706.855 us, Deactivation time : 0.491 us)), Write time : 4.949 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.751190203] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.752958608] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.753963963] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.754038745] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.754311483] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.764250954] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.767190920] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.767588205] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.767709786] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.770711971] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.771464371] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.774687324] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.776033723] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.776058350] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.777694970] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.792778190] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.793083204] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.795951489] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.798880154] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.799518321] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.802756815] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.804679343] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.804706885] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.804946751] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.817030312] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.818030457] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.820535522] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.821497990] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.825206983] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.826818400] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.826891930] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.827192852] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.837350557] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.848832494] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849504902] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849781107] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849812507] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.849838216] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.853181318] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.862805651] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409128.863711833] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409128.867325184] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409129.005576102] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 3059] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.260059673] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.260113485] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.260787582] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.284964681] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368481357] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368722636] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368764676] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.368781447] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.371023768] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.377663194] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.377726424] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.379264296] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.396826636] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.397933018] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.399216362] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.404014219] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.404059425] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.405038495] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409130.409930156] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.425512078] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.426795136] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.427700386] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.431824462] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.431866121] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.432687893] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.449164459] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.449616384] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.450960872] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409130.453804823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.558878 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.456153660] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.456179930] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.456369480] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.471233203] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.473173439] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.484151273] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.484200247] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.484707255] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.502925638] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.503308690] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.511922644] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.511970775] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.512378129] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3057] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.534818142] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.535604206] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.539977866] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.540009266] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.540606385] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_u_9iwkqc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409130.559346830] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409130.559769223] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 3060] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409131.686666050] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409131.686748396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409132.000946839] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.701065 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409133.877767277] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.521623 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.625747192] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.501367 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409137.102871533] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409137.102919725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409138.103064299] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.103116037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.291325368] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.079613 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409139.103057385] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.103107089] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.370623757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.377642 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409130.689048478] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409136.042658337] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:60489 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:60489 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:16] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.366806514] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.366867329] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.366882588] [moveit_2313487515.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.374051630] [moveit_2313487515.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.376068894] [moveit_2313487515.moveit.ros.rdf_loader]: Loaded robot model in 0.00183629 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.376110854] [moveit_2313487515.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.376121013] [moveit_2313487515.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409139.388832181] [moveit_2313487515.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.391833093] [moveit_2313487515.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.449507571] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.449650462] [moveit_2313487515.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.450663467] [moveit_2313487515.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451194186] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451209244] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451356975] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451800528] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.451899025] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.452779328] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.452794878] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.453258188] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.453767276] [moveit_2313487515.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409139.454022841] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409139.454063078] [moveit_2313487515.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.490527964] [moveit_2313487515.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.491455156] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.492869248] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.492886631] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493072474] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493085269] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493106850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493111198] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493121217] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.493867605] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.502151326] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.502165202] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.505305559] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.505319525] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.506323733] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.536448119] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.540276398] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.540541912] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.540566309] [moveit_2313487515.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.541600113] [moveit_2313487515.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:19] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:19] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409139.767143883] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:34 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:34 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:34 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409154.888994241] [moveit_2313487515.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:13 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_cylinder_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py::test_pick_up_cylinder_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 30.79s ========================= - - -12:32:45.72 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests -12:32:45.72 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-vnkI57 -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py::test_pick_up_cylinder_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:32:27 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-13-274212-DESKTOP-HG3Q5KV-3295 (testutils.py:33) - -2026-05-10 12:32:27 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [3309] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [3310] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [3311] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-4]: process started with pid [3312] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-5]: process started with pid [3313] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [robot_state_publisher-2] [INFO] [1778409133.616791812] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.620849729] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.626093281] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.629116406] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.629160610] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.629167643] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409133.629293988] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.792805024] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.795701517] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.796111216] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.796195686] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.805688448] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807031199] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807070965] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807102204] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807108697] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807211662] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409133.807270845] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409133.876987100] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.925254 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409133.962259061] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.215213049] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.215320754] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.218649739] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.239469781] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240405047] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240640404] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240705758] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.240734513] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.246172402] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.253283102] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.253322657] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.254378934] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.265071426] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.265334983] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.266198774] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.269233359] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.269286170] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.270174828] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.283118157] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.283457717] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.284038922] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.285851792] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.285892610] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.286579505] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.299158882] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.299602024] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.300175403] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.303461813] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.303501508] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.307538799] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.321246109] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.321600027] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.332091813] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.332152017] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.332474129] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.347624627] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.347992270] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.353576737] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.353617995] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.353902346] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.367456520] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.367776342] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.371653910] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.371689718] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.372203580] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-5] [INFO] [1778409134.385205807] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.385518926] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.509216011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 3313] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.761818659] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.761874085] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.763153055] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.779318682] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.780765777] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.780855036] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.786164915] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.788401190] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.791675356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.792764486] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.792809050] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.793171729] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.809111437] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.809467328] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.811063666] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409134.822501084] [controller_manager]: Overrun might occur, Total time : 10357.445 us (Expected < 2000.000 us) --> Read time : 17.353 us, Update time : 10335.984 us (Switch time : 10280.810 us (Switch chained mode time : 0.481 us, perform mode change time : 16.201 us, Activation time : 10247.947 us, Deactivation time : 0.250 us)), Write time : 4.108 us (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.822524744] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.825671342] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.826861334] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.826896611] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.827403560] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.840998085] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.843664427] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.844058370] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.844151497] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.846907254] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.848256608] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.851081740] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.852289982] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.852328054] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.853566097] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.871777795] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.872079714] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.874172155] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.877245365] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.878373352] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.881642650] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.883008720] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.883041903] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.883441232] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.901294841] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.901712810] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.905153839] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.906336226] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.909352647] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.910869614] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.910908127] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.911244697] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.925152314] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.939485053] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940198287] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940385423] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940402115] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.940409920] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.942166191] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.949263162] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409134.950472304] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [spawner-4] [INFO] [1778409134.953202953] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 3312] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [robot_state_publisher-2] [INFO] [1778409136.042665587] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3310] (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409136.344135290] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.344194302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.780264713] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.203308 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409136.806188164] [controller_manager]: Overrun might occur, Total time : 4055.797 us (Expected < 2000.000 us) --> Read time : 55.586 us, Update time : 3998.387 us, Write time : 1.824 us (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.291528799] [controller_manager]: Overrun might occur, Total time : 3372.678 us (Expected < 2000.000 us) --> Read time : 29.486 us, Update time : 3341.459 us, Write time : 1.733 us (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409138.291563585] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.502340 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.497881903] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.820507 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409139.612694550] [controller_manager]: Overrun might occur, Total time : 4574.598 us (Expected < 2000.000 us) --> Read time : 13.847 us, Update time : 4559.078 us, Write time : 1.673 us (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409142.320743429] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.682124 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409142.728495692] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409142.728555245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409143.145050259] [controller_manager]: Overrun might occur, Total time : 4905.335 us (Expected < 2000.000 us) --> Read time : 14.808 us, Update time : 4888.733 us, Write time : 1.794 us (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409143.728655558] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409143.728704812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409144.728891890] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409144.728940272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409145.728664622] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409145.728713004] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [INFO] [1778409146.728705568] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [ros2_control_node-1] [WARN] [1778409146.728752116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [1778409136.222518308] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] [INFO] [1778409147.353295221] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:32:27 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:21 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] * Running on http://127.0.0.1:52163 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] * Running on http://172.30.43.138:52163 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:22] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.045365582] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.045414214] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.045425586] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.050496542] [moveit_2895436883.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.052393977] [moveit_2895436883.moveit.ros.rdf_loader]: Loaded robot model in 0.00177331 seconds (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.052427250] [moveit_2895436883.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.052437039] [moveit_2895436883.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [WARN] [1778409145.064034325] [moveit_2895436883.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.066741860] [moveit_2895436883.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.124119435] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.124314852] [moveit_2895436883.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125197758] [moveit_2895436883.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125610662] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125625721] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.125751275] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126111399] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126196000] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126719174] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.126734153] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.127069861] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409145.127497764] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [WARN] [1778409145.127776724] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [ERROR] [1778409145.127793025] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.632426495] [moveit_2895436883.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.633152575] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634414122] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634428870] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634775629] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634791399] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634815064] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634824191] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.634837777] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.635684866] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.638001489] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.638015806] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.640038239] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.640053417] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.640822338] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.667587055] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.671061547] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.671220008] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.671241970] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409146.672056778] [moveit_2895436883.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:27] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:27] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409147.484945837] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:42 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:42 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] 2026-05-10 12:32:42 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:32:45 [ ERROR] [INFO] [1778409162.645452794] [moveit_2895436883.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -___________________ test_pick_up_cylinder_by_position[ur5e] ____________________ - -start_processes = Urls(ros_domain_id='54', robot_url='http://0.0.0.0:52163', storage_url='http://127.0.0.1:51465') - - @pytest.mark.timeout(321) - def test_pick_up_cylinder_by_position(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Cylinder("Cyl1", 0.1, 0.2) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Cyl1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-13-274212-DESKTOP-HG3Q5KV-3295 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [3309] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [3310] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [3311] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [3312] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [3313] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409133.616791812] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.620849729] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.626093281] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.629116406] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.629160610] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.629167643] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409133.629293988] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.792805024] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.795701517] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.796111216] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.796195686] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.805688448] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807031199] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807070965] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807102204] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807108697] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807211662] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409133.807270845] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409133.876987100] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.925254 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409133.962259061] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.215213049] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.215320754] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.218649739] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.239469781] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240405047] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240640404] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240705758] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.240734513] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.246172402] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.253283102] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.253322657] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.254378934] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.265071426] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.265334983] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.266198774] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.269233359] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.269286170] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.270174828] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.283118157] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.283457717] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.284038922] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.285851792] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.285892610] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.286579505] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.299158882] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.299602024] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.300175403] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.303461813] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.303501508] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.307538799] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.321246109] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.321600027] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.332091813] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.332152017] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.332474129] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.347624627] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.347992270] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.353576737] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.353617995] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.353902346] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.367456520] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.367776342] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.371653910] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.371689718] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.372203580] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409134.385205807] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.385518926] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.509216011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 3313] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.761818659] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.761874085] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.763153055] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.779318682] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.780765777] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.780855036] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.786164915] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.788401190] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.791675356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.792764486] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.792809050] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.793171729] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.809111437] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.809467328] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.811063666] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409134.822501084] [controller_manager]: Overrun might occur, Total time : 10357.445 us (Expected < 2000.000 us) --> Read time : 17.353 us, Update time : 10335.984 us (Switch time : 10280.810 us (Switch chained mode time : 0.481 us, perform mode change time : 16.201 us, Activation time : 10247.947 us, Deactivation time : 0.250 us)), Write time : 4.108 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.822524744] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.825671342] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.826861334] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.826896611] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.827403560] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.840998085] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.843664427] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.844058370] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.844151497] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.846907254] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.848256608] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.851081740] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.852289982] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.852328054] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.853566097] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.871777795] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.872079714] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.874172155] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.877245365] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.878373352] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.881642650] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.883008720] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.883041903] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.883441232] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.901294841] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.901712810] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.905153839] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.906336226] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.909352647] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.910869614] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.910908127] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.911244697] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_8i64_f8c -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.925152314] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.939485053] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940198287] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940385423] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940402115] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.940409920] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.942166191] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.949263162] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409134.950472304] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409134.953202953] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 3312] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409136.042665587] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 3310] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409136.344135290] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.344194302] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.780264713] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.203308 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409136.806188164] [controller_manager]: Overrun might occur, Total time : 4055.797 us (Expected < 2000.000 us) --> Read time : 55.586 us, Update time : 3998.387 us, Write time : 1.824 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.291528799] [controller_manager]: Overrun might occur, Total time : 3372.678 us (Expected < 2000.000 us) --> Read time : 29.486 us, Update time : 3341.459 us, Write time : 1.733 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409138.291563585] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.502340 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.497881903] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.820507 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409139.612694550] [controller_manager]: Overrun might occur, Total time : 4574.598 us (Expected < 2000.000 us) --> Read time : 13.847 us, Update time : 4559.078 us, Write time : 1.673 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409142.320743429] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.682124 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409142.728495692] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409142.728555245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409143.145050259] [controller_manager]: Overrun might occur, Total time : 4905.335 us (Expected < 2000.000 us) --> Read time : 14.808 us, Update time : 4888.733 us, Write time : 1.794 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409143.728655558] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409143.728704812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409144.728891890] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409144.728940272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409145.728664622] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409145.728713004] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409146.728705568] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409146.728752116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409136.222518308] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409147.353295221] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:21 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:52163 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:52163 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:22] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.045365582] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.045414214] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.045425586] [moveit_2895436883.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.050496542] [moveit_2895436883.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.052393977] [moveit_2895436883.moveit.ros.rdf_loader]: Loaded robot model in 0.00177331 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.052427250] [moveit_2895436883.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.052437039] [moveit_2895436883.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409145.064034325] [moveit_2895436883.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.066741860] [moveit_2895436883.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.124119435] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.124314852] [moveit_2895436883.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125197758] [moveit_2895436883.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125610662] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125625721] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.125751275] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126111399] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126196000] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126719174] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.126734153] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.127069861] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409145.127497764] [moveit_2895436883.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409145.127776724] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409145.127793025] [moveit_2895436883.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.632426495] [moveit_2895436883.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.633152575] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634414122] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634428870] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634775629] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634791399] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634815064] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634824191] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.634837777] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.635684866] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.638001489] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.638015806] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.640038239] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.640053417] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.640822338] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.667587055] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.671061547] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.671220008] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.671241970] [moveit_2895436883.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409146.672056778] [moveit_2895436883.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:27] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:27] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409147.484945837] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:42 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:42 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:42 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409162.645452794] [moveit_2895436883.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:13 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_cylinder_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py::test_pick_up_cylinder_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 32.59s ========================= - - -12:33:05.24 [INFO] Long running tasks: - 98.88s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 145.66s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:33:31.06 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests -12:33:31.07 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-2cCn2G -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py::test_side_box[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:33:12 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-47-039672-DESKTOP-HG3Q5KV-4225 (testutils.py:33) - -2026-05-10 12:33:12 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [4236] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [4237] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [4238] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-4]: process started with pid [4239] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-5]: process started with pid [4240] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [robot_state_publisher-2] [INFO] [1778409167.368767141] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.376216632] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.379285082] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.381617003] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.381652019] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.381658692] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409167.381768936] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.548425333] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.551539871] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.551960440] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.552027558] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.560850818] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562042542] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562079272] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562116733] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562128345] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562209108] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.562277067] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.669871827] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.922158493] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.922228706] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.924930449] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.937427295] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.938794644] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.938924630] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.947585902] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.948367793] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.951308836] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.952219081] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.952281169] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.958329120] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.971136807] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.971402192] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.972652276] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409167.983430315] [controller_manager]: Overrun might occur, Total time : 9314.058 us (Expected < 2000.000 us) --> Read time : 15.750 us, Update time : 9295.974 us (Switch time : 9244.916 us (Switch chained mode time : 0.361 us, perform mode change time : 14.077 us, Activation time : 9214.398 us, Deactivation time : 0.240 us)), Write time : 2.334 us (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409167.983481693] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.464490 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.983436507] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.984876984] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.985660713] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.985729383] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.986041025] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.995961426] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409167.997126515] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.997349324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.997391334] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409167.998885743] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.000216852] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.003112514] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.004042265] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.004087662] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.005100777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.019543896] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.019819950] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.021922681] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.022927605] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.024229413] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.027412227] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.028475371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.028508414] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.028873739] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.041398156] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.041674155] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.044810255] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.046271236] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.049042607] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.050094370] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.050116402] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.050365355] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.060861725] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.072491983] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.072795530] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.072989649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.073016019] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.073039955] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.075862769] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.085343383] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.086469564] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-4] [INFO] [1778409168.089348984] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.170792296] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 4239] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409168.406187520] [controller_manager]: Overrun might occur, Total time : 4089.955 us (Expected < 2000.000 us) --> Read time : 13.807 us, Update time : 4074.245 us, Write time : 1.903 us (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.426604695] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.426636415] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.426825575] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.455053647] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456165620] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456325333] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456362764] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.456378755] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.457817208] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.465826822] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.465911993] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.467745092] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.481287268] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.481614941] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.482681643] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.485626369] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.485664852] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.486438803] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.507184953] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.510981417] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.511444246] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.518886043] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.518905399] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.519490090] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.549536717] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.549811299] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.551288195] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.557313604] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.557333902] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.557475742] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.580935554] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.581397292] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.594499942] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.595726914] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.596324018] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.614528890] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.615091895] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.623864198] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.623892552] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.624327799] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.639394020] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.639769554] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.644060443] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.644085220] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.644700112] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [spawner-5] [INFO] [1778409168.662273334] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409168.662689786] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 4240] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [robot_state_publisher-2] [INFO] [1778409169.826882108] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4237] (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409170.164929478] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409170.164974995] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409172.094218999] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.202437 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409173.210273694] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.256591 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409173.376980454] [controller_manager]: Overrun might occur, Total time : 2885.692 us (Expected < 2000.000 us) --> Read time : 15.519 us, Update time : 2867.789 us, Write time : 2.384 us (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409176.022236848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.220066 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409176.505823894] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409176.505885230] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409177.506166203] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409177.506206189] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409178.506043014] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409178.506098440] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409179.506169548] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409179.506208712] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409180.258470979] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.453686 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409180.506121446] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409180.506174637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409181.277154462] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409181.505995471] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409181.506048071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409181.828258800] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409182.397555090] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409182.397612058] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409182.505980247] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409182.506033788] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409182.876183832] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.166979 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409183.506155943] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409183.506195989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409184.506221121] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409184.506281416] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409185.428444309] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.427427 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409185.506171015] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409185.506229647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409186.506239452] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409186.506283366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409187.506253267] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409187.506294405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409188.506163259] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409188.506201392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409189.506163987] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409189.506218821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409190.506213904] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409190.506259481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409191.506220124] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409191.506286711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [INFO] [1778409192.506246829] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [ros2_control_node-1] [WARN] [1778409192.506286795] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [1778409170.041558519] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] [INFO] [1778409192.774333495] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:33:12 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:32:55 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] * Running on http://127.0.0.1:47889 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] * Running on http://172.30.43.138:47889 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:32:55] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:32:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:32:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.406979936] [moveit_120247871.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.407039289] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.407054087] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.413780372] [moveit_120247871.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.415643377] [moveit_120247871.moveit.ros.rdf_loader]: Loaded robot model in 0.0017229 seconds (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.415670018] [moveit_120247871.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.415679776] [moveit_120247871.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [WARN] [1778409179.427128370] [moveit_120247871.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.430480400] [moveit_120247871.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.485061551] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.485175036] [moveit_120247871.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.485982200] [moveit_120247871.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.486587039] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.486601437] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.486710148] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487062127] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487129395] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487922633] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.487937411] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.488419487] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.488928384] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [WARN] [1778409179.489163661] [moveit_120247871.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [ERROR] [1778409179.489176846] [moveit_120247871.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.970445180] [moveit_120247871.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.971198601] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972382832] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972396758] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972746002] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972761151] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972793633] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972805625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.972818510] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.973512118] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.976069477] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.976084256] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.978056994] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.978070970] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409179.978742826] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.007873383] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.011961020] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.012179375] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.012247775] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409180.013074887] [moveit_120247871.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:01] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:01] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:02 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534388836] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534521899] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534534463] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.534551555] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [WARN] [1778409182.534646997] [moveit_120247871.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.535064079] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409182.535170241] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [ERROR] [1778409192.536104558] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [ERROR] [1778409192.536202013] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:12 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:12] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409192.903586513] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:27 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:27 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] 2026-05-10 12:33:27 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:33:30 [ ERROR] [INFO] [1778409208.044175737] [moveit_120247871.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________________ test_side_box[ur5e] ______________________________ - -start_processes = Urls(ros_domain_id='59', robot_url='http://0.0.0.0:47889', storage_url='http://127.0.0.1:34771') - - @pytest.mark.timeout(400) - def test_side_box(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -> ot.move_to_pose("", start_pose, 0.3, safe=False) - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-32-47-039672-DESKTOP-HG3Q5KV-4225 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [4236] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [4237] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [4238] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [4239] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [4240] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409167.368767141] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.376216632] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.379285082] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.381617003] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.381652019] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.381658692] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409167.381768936] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.548425333] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.551539871] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.551960440] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.552027558] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.560850818] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562042542] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562079272] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562116733] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562128345] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562209108] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.562277067] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.669871827] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.922158493] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.922228706] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.924930449] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.937427295] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.938794644] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.938924630] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.947585902] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.948367793] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.951308836] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.952219081] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.952281169] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.958329120] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.971136807] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.971402192] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.972652276] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409167.983430315] [controller_manager]: Overrun might occur, Total time : 9314.058 us (Expected < 2000.000 us) --> Read time : 15.750 us, Update time : 9295.974 us (Switch time : 9244.916 us (Switch chained mode time : 0.361 us, perform mode change time : 14.077 us, Activation time : 9214.398 us, Deactivation time : 0.240 us)), Write time : 2.334 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409167.983481693] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.464490 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.983436507] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.984876984] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.985660713] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.985729383] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.986041025] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.995961426] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409167.997126515] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.997349324] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.997391334] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409167.998885743] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.000216852] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.003112514] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.004042265] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.004087662] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.005100777] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.019543896] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.019819950] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.021922681] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.022927605] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.024229413] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.027412227] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.028475371] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.028508414] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.028873739] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.041398156] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.041674155] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.044810255] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.046271236] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.049042607] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.050094370] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.050116402] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.050365355] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.060861725] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.072491983] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.072795530] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.072989649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.073016019] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.073039955] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.075862769] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.085343383] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.086469564] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409168.089348984] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.170792296] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 4239] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409168.406187520] [controller_manager]: Overrun might occur, Total time : 4089.955 us (Expected < 2000.000 us) --> Read time : 13.807 us, Update time : 4074.245 us, Write time : 1.903 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.426604695] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.426636415] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.426825575] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.455053647] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456165620] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456325333] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456362764] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.456378755] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.457817208] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.465826822] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.465911993] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.467745092] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.481287268] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.481614941] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.482681643] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.485626369] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.485664852] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.486438803] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.507184953] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.510981417] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.511444246] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.518886043] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.518905399] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.519490090] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.549536717] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.549811299] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.551288195] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.557313604] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.557333902] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.557475742] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.580935554] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.581397292] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.594499942] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.595726914] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.596324018] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.614528890] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.615091895] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.623864198] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.623892552] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.624327799] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.639394020] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.639769554] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.644060443] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.644085220] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.644700112] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_eunh1kdr -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409168.662273334] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409168.662689786] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 4240] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409169.826882108] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4237] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409170.164929478] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409170.164974995] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409172.094218999] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.202437 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409173.210273694] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.256591 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409173.376980454] [controller_manager]: Overrun might occur, Total time : 2885.692 us (Expected < 2000.000 us) --> Read time : 15.519 us, Update time : 2867.789 us, Write time : 2.384 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409176.022236848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.220066 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409176.505823894] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409176.505885230] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409177.506166203] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409177.506206189] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409178.506043014] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409178.506098440] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409179.506169548] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409179.506208712] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409180.258470979] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.453686 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409180.506121446] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409180.506174637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409181.277154462] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409181.505995471] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409181.506048071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409181.828258800] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409182.397555090] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409182.397612058] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409182.505980247] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409182.506033788] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409182.876183832] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.166979 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409183.506155943] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409183.506195989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409184.506221121] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409184.506281416] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409185.428444309] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.427427 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409185.506171015] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409185.506229647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409186.506239452] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409186.506283366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409187.506253267] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409187.506294405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409188.506163259] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409188.506201392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409189.506163987] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409189.506218821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409190.506213904] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409190.506259481] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409191.506220124] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409191.506286711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409192.506246829] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409192.506286795] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409170.041558519] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409192.774333495] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:55 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:47889 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:47889 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:32:55] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:32:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.406979936] [moveit_120247871.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.407039289] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.407054087] [moveit_120247871.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.413780372] [moveit_120247871.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.415643377] [moveit_120247871.moveit.ros.rdf_loader]: Loaded robot model in 0.0017229 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.415670018] [moveit_120247871.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.415679776] [moveit_120247871.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409179.427128370] [moveit_120247871.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.430480400] [moveit_120247871.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.485061551] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.485175036] [moveit_120247871.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.485982200] [moveit_120247871.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.486587039] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.486601437] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.486710148] [moveit_120247871.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487062127] [moveit_120247871.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487129395] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487922633] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.487937411] [moveit_120247871.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.488419487] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.488928384] [moveit_120247871.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409179.489163661] [moveit_120247871.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409179.489176846] [moveit_120247871.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.970445180] [moveit_120247871.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.971198601] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972382832] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972396758] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972746002] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972761151] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972793633] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972805625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.972818510] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.973512118] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.976069477] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.976084256] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.978056994] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.978070970] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409179.978742826] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.007873383] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.011961020] [moveit_120247871.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.012179375] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.012247775] [moveit_120247871.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409180.013074887] [moveit_120247871.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:01] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:01] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:02 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534388836] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534521899] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534534463] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.534551555] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409182.534646997] [moveit_120247871.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.535064079] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409182.535170241] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409192.536104558] [moveit_120247871.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409192.536202013] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:12 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:12] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409192.903586513] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:27 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:27 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:27 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409208.044175737] [moveit_120247871.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:12 - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_box.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py::test_side_box[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed -======================== 1 failed, 1 warning in 44.25s ========================= - - -12:33:35.28 [INFO] Long running tasks: - 128.92s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 175.70s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:34:05.31 [INFO] Long running tasks: - 87.11s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - 158.95s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 205.73s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:34:06.45 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:34:21.24 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests -12:34:21.24 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-Ls8zBX -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py::test_side_sphere[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:34:02 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-33-32-171737-DESKTOP-HG3Q5KV-4936 (testutils.py:33) - -2026-05-10 12:34:02 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [4940] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [4941] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [4942] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-4]: process started with pid [4943] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-5]: process started with pid [4944] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [robot_state_publisher-2] [INFO] [1778409212.465490250] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.470039710] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.472588643] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.475083263] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.475118500] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.475125573] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409212.475233573] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.644206858] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.647442376] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.647898583] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.647982723] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.655995598] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657068182] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657101195] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657121193] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657126433] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657204582] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409212.657246271] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409212.748799587] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.000908490] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.000969957] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.003960965] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.024654190] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025597237] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025827645] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025872971] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.025903258] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.031551666] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.038643156] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.038686498] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.039817607] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.052856312] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.053112042] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.053831709] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.056540497] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.056571686] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.057088348] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.068549210] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.068784858] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.069281642] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.070844786] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.070879112] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.071388380] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.082585517] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.082846777] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.083368394] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.084638737] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.084673904] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.088308640] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.100682973] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.100902796] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.108838640] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.108874939] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.109125855] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.122856155] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.123115087] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.129043728] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.129080358] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.129391860] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.142519783] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.142785533] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.145271482] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.145313141] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.145864554] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-5] [INFO] [1778409213.158483411] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.158746200] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.262645702] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 4944] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.515020245] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.515069989] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.516412395] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.530817962] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.531750329] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.531822015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.536883723] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.537767072] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.540845588] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.541791454] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.541827021] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.542075473] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.556792297] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.557082083] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.558268578] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409213.567925492] [controller_manager]: Overrun might occur, Total time : 8375.094 us (Expected < 2000.000 us) --> Read time : 11.742 us, Update time : 8360.446 us (Switch time : 8318.877 us (Switch chained mode time : 0.411 us, perform mode change time : 9.408 us, Activation time : 8292.236 us, Deactivation time : 0.080 us)), Write time : 2.906 us (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409213.567984163] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.520516 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.567941738] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.570662618] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.571616601] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.571652499] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.572006171] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.584175594] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.586475765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.586795732] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.586856778] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.588459444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.589661608] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.592624648] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.593737647] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.593775339] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.595098248] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.610849323] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.611177986] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.613209107] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.614399258] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.615678578] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.618651166] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.619559879] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.619596899] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.620000450] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.634782338] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.635070140] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.638576938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.639673808] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.642587343] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.643750663] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.643784217] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.644153509] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.657719676] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.672795973] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673314999] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673428946] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673439566] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.673446229] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.674682878] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.680552903] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409213.681781527] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [spawner-4] [INFO] [1778409213.684818592] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 4943] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [robot_state_publisher-2] [INFO] [1778409214.967256777] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4941] (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409215.274382448] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409215.274432483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409217.191616263] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.153027 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409218.352173879] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.710332 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409221.182230906] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.767630 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409221.697781825] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409221.697829516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409222.697936231] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409222.697984483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409223.697890043] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409223.697941581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409224.697968400] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409224.698019137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409225.697992066] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409225.698064955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409226.697962898] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409226.698013093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409227.698016159] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409227.698070562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409228.697939558] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409228.697991076] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409229.698095101] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409229.698159694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409230.698014051] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409230.698065038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409230.931120946] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409231.482153959] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409231.697915581] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409231.697966989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409232.165232870] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409232.165296211] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409232.698032353] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409232.698086125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409233.698049344] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409233.698107665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409234.697999703] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409234.698052774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409235.698035235] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409235.698087935] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409236.698088704] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409236.698158177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409237.698092516] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409237.698172237] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409238.698042539] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409238.698096932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409239.698009296] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409239.698064781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409240.698182808] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409240.698258031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409241.698211266] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409241.698270107] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [INFO] [1778409242.698327532] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [ros2_control_node-1] [WARN] [1778409242.698385001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [1778409215.149691446] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] [INFO] [1778409242.791451833] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:34:02 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] * Running on http://127.0.0.1:58023 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] * Running on http://172.30.43.138:58023 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:40] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.935718815] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.935784039] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.935801002] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.942716633] [moveit_1973848282.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.944465907] [moveit_1973848282.moveit.ros.rdf_loader]: Loaded robot model in 0.00161206 seconds (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.944497417] [moveit_1973848282.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.944506444] [moveit_1973848282.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [WARN] [1778409223.955738676] [moveit_1973848282.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409223.958128406] [moveit_1973848282.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.012832016] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.012944970] [moveit_1973848282.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.013815695] [moveit_1973848282.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014312959] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014331895] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014457929] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014809006] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.014882216] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.015588619] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.015603848] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.016101844] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.016615060] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [WARN] [1778409224.016848834] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [ERROR] [1778409224.016859464] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.229328013] [moveit_1973848282.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.230404809] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.231901013] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.231917253] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232256628] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232271867] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232295302] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232299911] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.232315059] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.233124067] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.235685598] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.235700096] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.237529722] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.237542796] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.238212270] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.267385507] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.271213861] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.271365479] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.271387611] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409224.272191489] [moveit_1973848282.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:50] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:33:50] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:33:52 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441043840] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441166813] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441177443] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441194155] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [WARN] [1778409232.441294165] [moveit_1973848282.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441762796] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409232.441862155] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [ERROR] [1778409242.442338580] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [ERROR] [1778409242.442452246] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:02] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409242.922416753] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:17 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:17 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] 2026-05-10 12:34:17 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:34:20 [ ERROR] [INFO] [1778409258.043515185] [moveit_1973848282.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -____________________________ test_side_sphere[ur5e] ____________________________ - -start_processes = Urls(ros_domain_id='49', robot_url='http://0.0.0.0:58023', storage_url='http://127.0.0.1:40857') - - @pytest.mark.timeout(400) - def test_side_sphere(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -> ot.move_to_pose("", start_pose, 0.3, safe=False) - -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-33-32-171737-DESKTOP-HG3Q5KV-4936 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [4940] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [4941] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [4942] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [4943] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [4944] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409212.465490250] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.470039710] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.472588643] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.475083263] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.475118500] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.475125573] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409212.475233573] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.644206858] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.647442376] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.647898583] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.647982723] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.655995598] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657068182] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657101195] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657121193] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657126433] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657204582] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409212.657246271] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409212.748799587] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.000908490] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.000969957] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.003960965] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.024654190] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025597237] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025827645] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025872971] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.025903258] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.031551666] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.038643156] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.038686498] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.039817607] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.052856312] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.053112042] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.053831709] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.056540497] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.056571686] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.057088348] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.068549210] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.068784858] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.069281642] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.070844786] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.070879112] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.071388380] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.082585517] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.082846777] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.083368394] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.084638737] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.084673904] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.088308640] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.100682973] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.100902796] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.108838640] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.108874939] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.109125855] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.122856155] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.123115087] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.129043728] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.129080358] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.129391860] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.142519783] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.142785533] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.145271482] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.145313141] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.145864554] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409213.158483411] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.158746200] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.262645702] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 4944] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.515020245] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.515069989] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.516412395] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.530817962] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.531750329] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.531822015] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.536883723] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.537767072] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.540845588] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.541791454] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.541827021] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.542075473] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.556792297] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.557082083] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.558268578] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409213.567925492] [controller_manager]: Overrun might occur, Total time : 8375.094 us (Expected < 2000.000 us) --> Read time : 11.742 us, Update time : 8360.446 us (Switch time : 8318.877 us (Switch chained mode time : 0.411 us, perform mode change time : 9.408 us, Activation time : 8292.236 us, Deactivation time : 0.080 us)), Write time : 2.906 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409213.567984163] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.520516 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.567941738] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.570662618] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.571616601] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.571652499] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.572006171] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.584175594] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.586475765] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.586795732] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.586856778] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.588459444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.589661608] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.592624648] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.593737647] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.593775339] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.595098248] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.610849323] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.611177986] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.613209107] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.614399258] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.615678578] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.618651166] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.619559879] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.619596899] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.620000450] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.634782338] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.635070140] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.638576938] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.639673808] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.642587343] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.643750663] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.643784217] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.644153509] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_qtzge0_6 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.657719676] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.672795973] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673314999] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673428946] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673439566] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.673446229] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.674682878] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.680552903] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409213.681781527] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409213.684818592] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 4943] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409214.967256777] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 4941] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409215.274382448] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409215.274432483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409217.191616263] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.153027 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409218.352173879] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.710332 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409221.182230906] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.767630 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409221.697781825] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409221.697829516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409222.697936231] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409222.697984483] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409223.697890043] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409223.697941581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409224.697968400] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409224.698019137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409225.697992066] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409225.698064955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409226.697962898] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409226.698013093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409227.698016159] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409227.698070562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409228.697939558] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409228.697991076] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409229.698095101] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409229.698159694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409230.698014051] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409230.698065038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409230.931120946] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409231.482153959] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409231.697915581] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409231.697966989] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409232.165232870] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409232.165296211] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409232.698032353] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409232.698086125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409233.698049344] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409233.698107665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409234.697999703] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409234.698052774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409235.698035235] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409235.698087935] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409236.698088704] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409236.698158177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409237.698092516] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409237.698172237] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409238.698042539] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409238.698096932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409239.698009296] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409239.698064781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409240.698182808] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409240.698258031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409241.698211266] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409241.698270107] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409242.698327532] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409242.698385001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409215.149691446] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409242.791451833] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:58023 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:58023 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:40] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.935718815] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.935784039] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.935801002] [moveit_1973848282.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.942716633] [moveit_1973848282.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.944465907] [moveit_1973848282.moveit.ros.rdf_loader]: Loaded robot model in 0.00161206 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.944497417] [moveit_1973848282.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.944506444] [moveit_1973848282.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409223.955738676] [moveit_1973848282.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409223.958128406] [moveit_1973848282.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.012832016] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.012944970] [moveit_1973848282.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.013815695] [moveit_1973848282.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014312959] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014331895] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014457929] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014809006] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.014882216] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.015588619] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.015603848] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.016101844] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.016615060] [moveit_1973848282.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409224.016848834] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409224.016859464] [moveit_1973848282.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.229328013] [moveit_1973848282.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.230404809] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.231901013] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.231917253] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232256628] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232271867] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232295302] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232299911] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.232315059] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.233124067] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.235685598] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.235700096] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.237529722] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.237542796] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.238212270] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.267385507] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.271213861] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.271365479] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.271387611] [moveit_1973848282.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409224.272191489] [moveit_1973848282.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:50] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:33:50] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:33:52 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441043840] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441166813] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441177443] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441194155] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409232.441294165] [moveit_1973848282.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441762796] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409232.441862155] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409242.442338580] [moveit_1973848282.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409242.442452246] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:02] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409242.922416753] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:17 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:17 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:17 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409258.043515185] [moveit_1973848282.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:12 - src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:12: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_side_obstacle_sphere.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py::test_side_sphere[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed -======================== 1 failed, 1 warning in 49.27s ========================= - - -12:34:30.63 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:34:35.35 [INFO] Long running tasks: - 117.15s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:34:40.11 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests -12:34:40.12 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-0WZ8YU -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py::test_pick_up_sphere_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:34:21 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-07-459230-DESKTOP-HG3Q5KV-5452 (testutils.py:33) - -2026-05-10 12:34:21 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [5456] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [5457] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [5458] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-4]: process started with pid [5459] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-5]: process started with pid [5460] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778409247.718179587] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.722477102] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.725155402] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.727550032] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.727580860] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.727586371] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409247.727680149] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.735680410] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.738127049] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.738569500] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.738644001] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.746282490] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747347785] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747375728] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747399924] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747404332] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747452023] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409247.747484094] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409247.984163459] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.236373294] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.236437406] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.240143818] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.263055593] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264072475] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264284238] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264327360] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.264348590] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.268535075] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.275118263] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.275160533] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.276230046] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.286949794] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.287175959] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.287935211] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.289141238] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.289176044] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.289703035] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.302839507] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.303071247] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.303560427] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.305130099] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.305165827] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.305819238] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.319249907] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.319473031] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.319961540] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.321377240] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.321418328] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.325251522] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.337077302] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.337318971] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.345490574] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.345533416] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.345798389] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.361151860] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.361392603] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.367441107] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.367470803] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.367723924] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.381147631] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.381475423] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.385845887] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.385884811] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.386579671] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-5] [INFO] [1778409248.399239210] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.399572830] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.498576222] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 5460] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.751130986] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.751185249] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.752508879] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.767162819] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.768239129] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.768306688] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.773571887] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.774377999] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.776988368] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.777876636] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.777912484] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.778299844] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.790909834] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.791180213] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.792680100] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409248.802300875] [controller_manager]: Overrun might occur, Total time : 8247.351 us (Expected < 2000.000 us) --> Read time : 12.794 us, Update time : 8231.912 us (Switch time : 8186.696 us (Switch chained mode time : 0.260 us, perform mode change time : 9.778 us, Activation time : 8161.538 us, Deactivation time : 0.070 us)), Write time : 2.645 us (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409248.802355418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.375299 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.802318158] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.805132505] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.806146065] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.806189057] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.806537299] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.818062554] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.819097761] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.819360129] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.819414292] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.822961893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.824198057] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.827157479] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.828392261] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.828427878] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.829453728] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.847062436] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.847281442] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.849129438] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.850740490] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.852161780] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.855007397] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.855990320] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.856029715] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.856298861] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.873067663] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.873402555] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.876886920] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.878233995] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.881185614] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.882362635] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.882398072] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.882685438] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.897234809] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.911151622] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911469020] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911664351] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911710598] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.911723834] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.913298616] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.919321717] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409248.920338297] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [spawner-4] [INFO] [1778409248.923605826] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 5459] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [robot_state_publisher-2] [INFO] [1778409250.274436007] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5457] (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409250.552126363] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409250.552181568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409252.481055497] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.075528 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409253.627411756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.431847 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409256.594211181] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.231432 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409256.897872969] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409256.897918215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409257.200541906] [controller_manager]: Overrun might occur, Total time : 2499.329 us (Expected < 2000.000 us) --> Read time : 13.536 us, Update time : 2484.210 us, Write time : 1.583 us (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409257.898023018] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409257.898066320] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409258.898193064] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409258.898238501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409259.898108052] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409259.898203262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [INFO] [1778409260.898187361] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409260.898220494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [ros2_control_node-1] [WARN] [1778409261.273475272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.495503 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [1778409250.440024799] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] [INFO] [1778409261.373104140] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:34:21 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] * Running on http://127.0.0.1:46689 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] * Running on http://172.30.43.138:46689 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:16] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:19 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.099363680] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.099415108] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.099431409] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.103312629] [moveit_3975733361.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.105138488] [moveit_3975733361.moveit.ros.rdf_loader]: Loaded robot model in 0.00169144 seconds (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.105167072] [moveit_3975733361.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.105175428] [moveit_3975733361.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [WARN] [1778409260.117255207] [moveit_3975733361.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.120208057] [moveit_3975733361.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.174133451] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.174351330] [moveit_3975733361.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175204942] [moveit_3975733361.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175544628] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175558344] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175657622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175879644] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.175956901] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.176519622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.176534690] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.177092480] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.177416159] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [WARN] [1778409260.177628613] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [ERROR] [1778409260.177643361] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.450338013] [moveit_3975733361.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.451324102] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.452727889] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.452742968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453070911] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453086109] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453116577] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453126827] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.453140593] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.454045572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.456394001] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.456410182] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.458842353] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.458858554] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.459469004] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.487247385] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.489924833] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.490087843] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.490117600] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409260.490854400] [moveit_3975733361.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:21] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:21] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409261.534614756] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:36 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:36 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] 2026-05-10 12:34:36 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:34:39 [ ERROR] [INFO] [1778409276.674727546] [moveit_3975733361.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_______________________ test_pick_up_sphere_by_id[ur5e] ________________________ - -start_processes = Urls(ros_domain_id='132', robot_url='http://0.0.0.0:46689', storage_url='http://127.0.0.1:59497') - - @pytest.mark.timeout(321) - def test_pick_up_sphere_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Sphere("Sphere1", 0.1) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Sphere1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-07-459230-DESKTOP-HG3Q5KV-5452 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [5456] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [5457] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [5458] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [5459] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [5460] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409247.718179587] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.722477102] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.725155402] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.727550032] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.727580860] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.727586371] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409247.727680149] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.735680410] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.738127049] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.738569500] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.738644001] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.746282490] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747347785] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747375728] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747399924] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747404332] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747452023] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409247.747484094] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409247.984163459] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.236373294] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.236437406] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.240143818] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.263055593] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264072475] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264284238] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264327360] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.264348590] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.268535075] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.275118263] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.275160533] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.276230046] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.286949794] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.287175959] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.287935211] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.289141238] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.289176044] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.289703035] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.302839507] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.303071247] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.303560427] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.305130099] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.305165827] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.305819238] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.319249907] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.319473031] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.319961540] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.321377240] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.321418328] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.325251522] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.337077302] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.337318971] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.345490574] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.345533416] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.345798389] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.361151860] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.361392603] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.367441107] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.367470803] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.367723924] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.381147631] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.381475423] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.385845887] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.385884811] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.386579671] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409248.399239210] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.399572830] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.498576222] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 5460] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.751130986] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.751185249] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.752508879] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.767162819] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.768239129] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.768306688] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.773571887] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.774377999] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.776988368] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.777876636] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.777912484] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.778299844] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.790909834] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.791180213] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.792680100] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409248.802300875] [controller_manager]: Overrun might occur, Total time : 8247.351 us (Expected < 2000.000 us) --> Read time : 12.794 us, Update time : 8231.912 us (Switch time : 8186.696 us (Switch chained mode time : 0.260 us, perform mode change time : 9.778 us, Activation time : 8161.538 us, Deactivation time : 0.070 us)), Write time : 2.645 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409248.802355418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.375299 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.802318158] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.805132505] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.806146065] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.806189057] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.806537299] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.818062554] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.819097761] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.819360129] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.819414292] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.822961893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.824198057] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.827157479] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.828392261] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.828427878] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.829453728] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.847062436] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.847281442] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.849129438] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.850740490] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.852161780] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.855007397] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.855990320] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.856029715] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.856298861] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.873067663] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.873402555] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.876886920] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.878233995] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.881185614] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.882362635] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.882398072] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.882685438] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_hgox96bt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.897234809] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.911151622] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911469020] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911664351] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911710598] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.911723834] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.913298616] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.919321717] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409248.920338297] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409248.923605826] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 5459] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409250.274436007] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5457] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409250.552126363] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409250.552181568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409252.481055497] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.075528 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409253.627411756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.431847 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409256.594211181] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.231432 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409256.897872969] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409256.897918215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409257.200541906] [controller_manager]: Overrun might occur, Total time : 2499.329 us (Expected < 2000.000 us) --> Read time : 13.536 us, Update time : 2484.210 us, Write time : 1.583 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409257.898023018] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409257.898066320] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409258.898193064] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409258.898238501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409259.898108052] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409259.898203262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409260.898187361] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409260.898220494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409261.273475272] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.495503 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409250.440024799] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409261.373104140] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:15 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:46689 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:46689 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:16] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:19 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.099363680] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.099415108] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.099431409] [moveit_3975733361.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.103312629] [moveit_3975733361.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.105138488] [moveit_3975733361.moveit.ros.rdf_loader]: Loaded robot model in 0.00169144 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.105167072] [moveit_3975733361.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.105175428] [moveit_3975733361.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409260.117255207] [moveit_3975733361.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.120208057] [moveit_3975733361.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.174133451] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.174351330] [moveit_3975733361.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175204942] [moveit_3975733361.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175544628] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175558344] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175657622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175879644] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.175956901] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.176519622] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.176534690] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.177092480] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.177416159] [moveit_3975733361.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409260.177628613] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409260.177643361] [moveit_3975733361.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.450338013] [moveit_3975733361.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.451324102] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.452727889] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.452742968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453070911] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453086109] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453116577] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453126827] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.453140593] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.454045572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.456394001] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.456410182] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.458842353] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.458858554] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.459469004] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.487247385] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.489924833] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.490087843] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.490117600] [moveit_3975733361.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409260.490854400] [moveit_3975733361.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:21] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:21] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409261.534614756] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409276.674727546] [moveit_3975733361.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:13 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_sphere_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py::test_pick_up_sphere_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 32.66s ========================= - - -12:35:05.43 [INFO] Long running tasks: - 147.23s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:35:24.52 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests -12:35:24.52 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-qZ6cxi -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py::test_graspable[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:35:05 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-41-439940-DESKTOP-HG3Q5KV-6668 (testutils.py:33) - -2026-05-10 12:35:05 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [6698] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [6699] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [6700] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-4]: process started with pid [6701] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-5]: process started with pid [6702] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.788077567] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778409281.790567232] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.791061877] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.793577958] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.793596723] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.793602304] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.793840210] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.801718947] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.803914924] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.804282773] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.804388795] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.811330154] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812545584] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812576292] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812596070] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812601430] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812655683] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.812696020] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.106808994] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.359004917] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.359071063] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.362008891] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.375328284] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.376171476] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.376308717] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.383658091] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.384475163] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.387522603] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.388368942] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.388400271] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.392839857] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.405484560] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.405788045] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.406771840] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.418864822] [controller_manager]: Overrun might occur, Total time : 10708.702 us (Expected < 2000.000 us) --> Read time : 17.022 us, Update time : 10687.602 us (Switch time : 10659.379 us (Switch chained mode time : 0.301 us, perform mode change time : 15.189 us, Activation time : 10641.204 us, Deactivation time : 0.330 us)), Write time : 4.078 us (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.418904347] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.824074 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.418931489] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.421680683] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.422686204] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.422759052] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.423179902] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.432910010] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.435649345] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.436172419] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.436221412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.437850133] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.438255439] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.441295459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.442448279] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.442470311] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.443601631] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.457416671] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.457624290] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.460011256] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.462904895] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.464276230] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.467412930] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.468604931] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.468635659] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.468886576] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.481635059] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.481917581] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.485045504] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.486313995] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.489154231] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.490247568] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.490270352] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.490582260] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.502172287] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.513709168] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514776758] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514933927] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514959475] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.514979664] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.517342523] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.523326522] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.524553986] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-4] [INFO] [1778409282.527628146] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.627385753] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 6701] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.879925905] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.880009664] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.880302600] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.901516269] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903344929] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903536213] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903558204] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.903567642] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.904886202] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.911907710] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.911965489] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.913369391] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.927611302] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.927946238] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.928755316] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.931599731] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.931636631] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.932432699] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.947418829] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.948195314] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.948789934] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.951893316] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.951917111] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.952762142] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.965632577] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.965955641] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.966475368] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.970015070] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.970037873] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.970381772] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409282.983224674] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.983524985] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.993827066] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.993919632] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.994168906] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409283.009736646] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.010110711] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.015439542] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.015480069] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.015774759] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409283.031354903] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.031675607] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.036049323] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.036073238] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.036402915] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [spawner-5] [INFO] [1778409283.055820295] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.056323001] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 6702] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409283.882227362] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.146327 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [robot_state_publisher-2] [INFO] [1778409284.188563654] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6699] (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409284.542105761] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409284.542161126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.136269142] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.188288 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.318366848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.286294 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.372122650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.042577 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.410296108] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.215584 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.754266594] [controller_manager]: Overrun might occur, Total time : 2121.971 us (Expected < 2000.000 us) --> Read time : 15.970 us, Update time : 2104.088 us, Write time : 1.913 us (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.450336154] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.255950 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409291.014211003] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.014276738] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.460968420] [controller_manager]: Overrun might occur, Total time : 4794.045 us (Expected < 2000.000 us) --> Read time : 34.947 us, Update time : 4756.573 us, Write time : 2.525 us (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.461015790] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.935306 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409292.014011352] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.014060736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409293.014057340] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.014095903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.886275650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.195266 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.014882028] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.014953859] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.338521827] [io_and_status_controller]: Setting speed slider to 30.00%. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.889535425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.013973206] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.014030104] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.459765375] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.459807775] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409296.014038170] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.014091491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.454290217] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.209833 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409297.014083513] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409297.014162393] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409298.014047494] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.014095826] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.199527048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.446714 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409299.013913916] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409299.013963600] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409300.013975980] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.014041103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.269611608] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.530994 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409301.014053707] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409301.014109062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409302.014248366] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409302.014302429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409303.014067599] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409303.014145978] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409304.013980739] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409304.014069378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [INFO] [1778409305.014351460] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.014405883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.350305056] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.224703 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [1778409284.433083616] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:35:05 [ ERROR] [INFO] [1778409305.873138648] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:06 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:35:06 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:50 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] * Running on http://127.0.0.1:50169 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] * Running on http://172.30.43.138:50169 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:50] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.731355588] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.731395063] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.731405563] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.738733317] [moveit_1824579522.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.740921594] [moveit_1824579522.moveit.ros.rdf_loader]: Loaded robot model in 0.0020249 seconds (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.740952092] [moveit_1824579522.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.740959727] [moveit_1824579522.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [WARN] [1778409293.755298157] [moveit_1824579522.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.758242526] [moveit_1824579522.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.821791196] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.821915973] [moveit_1824579522.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.822937144] [moveit_1824579522.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.823504803] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.823519110] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.823649277] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824136223] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824198872] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824719180] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.824731123] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.825424696] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.825984981] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [WARN] [1778409293.826311371] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [ERROR] [1778409293.826332812] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.877052195] [moveit_1824579522.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.877965430] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879417850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879435122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879733188] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879746494] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879799694] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879815625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.879833940] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.880716226] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.883475879] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.883493232] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.886432112] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.886447632] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.887341986] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.918199460] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.922162563] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.922366501] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.922404583] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409293.923247615] [moveit_1824579522.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:54] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:54] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:34:55 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471068818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471179549] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471189337] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471205438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [WARN] [1778409295.471300959] [moveit_1824579522.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471711048] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409295.471842789] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [ERROR] [1778409305.472574677] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [ERROR] [1778409305.472682733] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:05 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] result = runtime.move_to_pose( (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] plan_and_execute(moveitpy, ur_manipulator, logger) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] raise UrGeneral("Planning failed") (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] arcor2_ur.exceptions.UrGeneral: Planning failed (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:05] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409306.002848988] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:21 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:21 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] 2026-05-10 12:35:21 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:35:24 [ ERROR] [INFO] [1778409321.171936987] [moveit_1824579522.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________________ test_graspable[ur5e] _____________________________ - -start_processes = Urls(ros_domain_id='101', robot_url='http://0.0.0.0:50169', storage_url='http://127.0.0.1:50735') - - @pytest.mark.timeout(400) - def test_graspable(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.5 - Y = 0.0 - Z = 0.3 -  - start_pose = Pose(Position(X, Y, Z), Orientation(1, 0, 0, 0)) - goal_pose = Pose(Position(Y, X, Z), Orientation(1, 0, 0, 0)) -> ot.move_to_pose("", start_pose, 0.3, safe=False) - -src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:28: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:120: in move_to_pose - self.move( -src/python/arcor2_ur/object_types/ur5e.py:149: in move - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-41-439940-DESKTOP-HG3Q5KV-6668 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [6698] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [6699] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [6700] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [6701] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [6702] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.788077567] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409281.790567232] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.791061877] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.793577958] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.793596723] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.793602304] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.793840210] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.801718947] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.803914924] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.804282773] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.804388795] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.811330154] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812545584] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812576292] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812596070] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812601430] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812655683] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.812696020] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.106808994] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.359004917] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.359071063] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.362008891] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.375328284] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.376171476] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.376308717] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.383658091] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.384475163] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.387522603] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.388368942] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.388400271] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.392839857] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.405484560] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.405788045] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.406771840] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.418864822] [controller_manager]: Overrun might occur, Total time : 10708.702 us (Expected < 2000.000 us) --> Read time : 17.022 us, Update time : 10687.602 us (Switch time : 10659.379 us (Switch chained mode time : 0.301 us, perform mode change time : 15.189 us, Activation time : 10641.204 us, Deactivation time : 0.330 us)), Write time : 4.078 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.418904347] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.824074 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.418931489] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.421680683] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.422686204] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.422759052] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.423179902] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.432910010] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.435649345] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.436172419] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.436221412] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.437850133] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.438255439] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.441295459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.442448279] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.442470311] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.443601631] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.457416671] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.457624290] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.460011256] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.462904895] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.464276230] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.467412930] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.468604931] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.468635659] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.468886576] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.481635059] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.481917581] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.485045504] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.486313995] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.489154231] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.490247568] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.490270352] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.490582260] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.502172287] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.513709168] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514776758] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514933927] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514959475] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.514979664] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.517342523] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.523326522] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.524553986] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409282.527628146] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.627385753] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 6701] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.879925905] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.880009664] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.880302600] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.901516269] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903344929] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903536213] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903558204] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.903567642] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.904886202] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.911907710] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.911965489] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.913369391] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.927611302] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.927946238] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.928755316] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.931599731] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.931636631] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.932432699] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.947418829] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.948195314] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.948789934] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.951893316] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.951917111] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.952762142] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.965632577] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.965955641] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.966475368] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.970015070] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.970037873] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.970381772] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409282.983224674] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.983524985] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.993827066] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.993919632] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.994168906] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409283.009736646] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.010110711] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.015439542] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.015480069] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.015774759] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409283.031354903] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.031675607] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.036049323] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.036073238] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.036402915] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_mxld1hpe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409283.055820295] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.056323001] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 6702] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409283.882227362] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.146327 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409284.188563654] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6699] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409284.542105761] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409284.542161126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.136269142] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.188288 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.318366848] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.286294 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.372122650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.042577 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.410296108] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.215584 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.754266594] [controller_manager]: Overrun might occur, Total time : 2121.971 us (Expected < 2000.000 us) --> Read time : 15.970 us, Update time : 2104.088 us, Write time : 1.913 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.450336154] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.255950 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409291.014211003] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.014276738] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.460968420] [controller_manager]: Overrun might occur, Total time : 4794.045 us (Expected < 2000.000 us) --> Read time : 34.947 us, Update time : 4756.573 us, Write time : 2.525 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.461015790] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.935306 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409292.014011352] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.014060736] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409293.014057340] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.014095903] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.886275650] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.195266 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.014882028] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.014953859] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.338521827] [io_and_status_controller]: Setting speed slider to 30.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.889535425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.013973206] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.014030104] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.459765375] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.459807775] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409296.014038170] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.014091491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.454290217] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.209833 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409297.014083513] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409297.014162393] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409298.014047494] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.014095826] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.199527048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.446714 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409299.013913916] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409299.013963600] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409300.013975980] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.014041103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.269611608] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.530994 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409301.014053707] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409301.014109062] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409302.014248366] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409302.014302429] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409303.014067599] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409303.014145978] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409304.013980739] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409304.014069378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409305.014351460] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.014405883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.350305056] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.224703 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409284.433083616] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409305.873138648] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:50169 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:50169 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:50] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.731355588] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.731395063] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.731405563] [moveit_1824579522.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.738733317] [moveit_1824579522.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.740921594] [moveit_1824579522.moveit.ros.rdf_loader]: Loaded robot model in 0.0020249 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.740952092] [moveit_1824579522.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.740959727] [moveit_1824579522.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409293.755298157] [moveit_1824579522.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.758242526] [moveit_1824579522.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.821791196] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.821915973] [moveit_1824579522.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.822937144] [moveit_1824579522.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.823504803] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.823519110] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.823649277] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824136223] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824198872] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824719180] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.824731123] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.825424696] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.825984981] [moveit_1824579522.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409293.826311371] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409293.826332812] [moveit_1824579522.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.877052195] [moveit_1824579522.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.877965430] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879417850] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879435122] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879733188] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879746494] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879799694] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879815625] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.879833940] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.880716226] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.883475879] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.883493232] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.886432112] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.886447632] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.887341986] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.918199460] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.922162563] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.922366501] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.922404583] [moveit_1824579522.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409293.923247615] [moveit_1824579522.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:54] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:54] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471068818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471179549] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471189337] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471205438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409295.471300959] [moveit_1824579522.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471711048] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409295.471842789] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409305.472574677] [moveit_1824579522.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409305.472682733] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1166, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.move_to_pose( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 730, in move_to_pose -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 plan_and_execute(moveitpy, ur_manipulator, logger) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 168, in plan_and_execute -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Planning failed") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Planning failed -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:05] "PUT /eef/pose?velocity=30.0&payload=0.0&safe=false HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409306.002848988] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.171936987] [moveit_1824579522.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:13 - src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_collision.test_obstacle_graspable.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py::test_graspable[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Planning failed -======================== 1 failed, 1 warning in 43.36s ========================= - - -12:35:35.47 [INFO] Long running tasks: - 64.84s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests - 74.24s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests - 177.27s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:36:01.41 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests -12:36:01.42 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-cU2fDa -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py::test_pick_and_place_box_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:35:43 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-22-272481-DESKTOP-HG3Q5KV-5908 (testutils.py:33) - -2026-05-10 12:35:43 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [5912] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [5913] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [5914] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-4]: process started with pid [5915] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-5]: process started with pid [5916] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [robot_state_publisher-2] [INFO] [1778409262.549551434] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.550457761] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.553354602] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.555612692] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.555644413] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.555651045] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409262.555761630] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.729985698] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.732621687] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.732998733] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.733066853] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.741374118] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742687063] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742721548] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742745754] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742751355] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742805768] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409262.742844001] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409262.803210390] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.055317185] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.055374063] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.058681608] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.080953021] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.081839772] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.081992482] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.082034442] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.082059630] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.086465710] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.093388224] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.093430134] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.094539742] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.104989910] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.105207317] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.105966290] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.107140651] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.107177591] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.107740130] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.119096047] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.119339585] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.119954905] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.121406112] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.121440568] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.121960365] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.133002356] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.133243524] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.133744436] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.134985885] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.135016714] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.138622150] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.151262243] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.151503941] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.159180969] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.159218540] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.159600185] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.171009680] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.171267054] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.177676538] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.177711404] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.178019029] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.191023910] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.191270394] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.193976871] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.194014963] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.194618209] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-5] [INFO] [1778409263.206927329] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.207167320] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.308475819] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 5916] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.560625400] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.560675385] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.561966084] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.575637796] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.576889119] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.576951507] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.581845691] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.582339024] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.585310080] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.586413045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.586448994] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.586850907] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.600894532] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.601178251] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.602805127] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409263.612952985] [controller_manager]: Overrun might occur, Total time : 8898.944 us (Expected < 2000.000 us) --> Read time : 11.702 us, Update time : 8884.737 us (Switch time : 8840.524 us (Switch chained mode time : 0.441 us, perform mode change time : 9.799 us, Activation time : 8813.131 us, Deactivation time : 0.140 us)), Write time : 2.505 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409263.613015333] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.037376 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.612969140] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.615170858] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.616300826] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.616341373] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.616779290] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.628921080] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.631006789] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.631341751] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.631401394] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.634990744] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.636187353] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.639094170] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.640163719] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.640199907] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.641309345] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.657238744] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.657506522] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.659464733] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.660829135] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.662209137] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.665137747] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.666263536] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.666306458] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.666634035] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.680954336] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.681254321] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.684966910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.686215437] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.689020031] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.690004677] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.690041317] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.690488281] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.703309739] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.717557506] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718113307] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718266868] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718282738] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.718289942] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.719909455] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.726856455] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409263.728363854] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [spawner-4] [INFO] [1778409263.731018569] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 5915] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [robot_state_publisher-2] [INFO] [1778409265.094024084] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5913] (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409265.387438825] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409265.387482358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409267.302218411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.240925 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409268.521119103] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.141647 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409270.655726791] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.749215 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409271.917770519] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409271.917824671] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409272.238216327] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.238751 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.854995613] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409272.855053884] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.855185279] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409273.855238721] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409274.855103029] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409274.855145400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409275.855039112] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409275.855091342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409276.855195118] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409276.855249081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.018341836] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 12.364320 ms (missed cycles : 7). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.060080729] [controller_manager]: Overrun might occur, Total time : 2015.961 us (Expected < 2000.000 us) --> Read time : 1952.540 us, Update time : 61.668 us, Write time : 1.753 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409277.855301274] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.855699701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409277.892541089] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.225895372] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.917836 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.444744492] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409278.855168922] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.855225119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409279.008537622] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.008582166] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.051281405] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.051343844] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409279.855302616] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409279.855345318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409280.124092009] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.114303 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409280.316084873] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409280.855014189] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409280.855058113] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.126164413] [controller_manager]: Overrun might occur, Total time : 2128.183 us (Expected < 2000.000 us) --> Read time : 14.557 us, Update time : 2111.993 us, Write time : 1.633 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.126206313] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.228766 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.855296851] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.855377734] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.855067697] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.855105298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.855059245] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409283.855114179] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409284.854982896] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409284.855040505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409285.855018714] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409285.855073298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.139390759] [controller_manager]: Overrun might occur, Total time : 3316.512 us (Expected < 2000.000 us) --> Read time : 3181.095 us, Update time : 133.043 us, Write time : 2.374 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.139435113] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.457296 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409286.855389070] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.855429177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.678614592] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.636926 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409287.855142507] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.855653993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.856549159] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.856596519] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.386304226] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.326620 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409289.855153616] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.855191267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.450298411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.320805 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409290.854903404] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.854956374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.474354175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.376739 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409291.854858784] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.854933897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.765276491] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.298504 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.772776262] [controller_manager]: Overrun might occur, Total time : 2990.028 us (Expected < 2000.000 us) --> Read time : 28.615 us, Update time : 2958.677 us, Write time : 2.736 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409292.854952221] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.855007917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409293.855045529] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.855097668] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.906289378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.311942 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.962254353] [controller_manager]: Overrun might occur, Total time : 2172.272 us (Expected < 2000.000 us) --> Read time : 20.940 us, Update time : 2148.837 us, Write time : 2.495 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.854928226] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.854985916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.854884480] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.854938803] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409296.854941558] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.854985982] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409297.854830332] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409297.854896297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.217584925] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.607228 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409298.854972974] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.855014423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409299.854979937] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409299.855036505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409300.854935218] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.854998027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409301.855057655] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409301.855111458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409302.854942451] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409302.854998348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409303.855037568] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409303.855097532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409304.854979272] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409304.855039496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409305.855825209] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.855870064] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409306.854886888] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409306.854939358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409307.854910257] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409307.854979268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409308.854918743] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409308.854973277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409309.854883406] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409309.854941236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409310.462073398] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409310.854897887] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409310.854945647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.013138704] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.586948576] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409311.587046582] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409311.854893328] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.854946479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.160861944] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.160926577] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.754086783] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.854867122] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409312.854908330] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409313.854914692] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409313.854968284] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409314.854924200] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409314.854977781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409315.855092087] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409315.855140990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409316.855040709] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409316.855096445] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409317.855064449] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409317.855114585] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409318.855057005] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409318.855111879] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409319.855238653] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409319.855305129] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409320.854835531] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409320.854886108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.855049995] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409321.855124316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409322.855096098] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409322.855149469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409323.855141204] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409323.855192321] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.738210221] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.232475 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.804632913] [controller_manager]: Overrun might occur, Total time : 2596.895 us (Expected < 2000.000 us) --> Read time : 13.135 us, Update time : 2582.237 us, Write time : 1.523 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409324.854939402] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.854985470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.855082794] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409325.855139452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.855056084] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409326.855109715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.854848193] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.854900312] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409328.855001595] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409328.855048754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409329.855052657] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409329.855100838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.678223647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.246081 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409330.855031462] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.855082169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.858219359] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.241863 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409331.858556786] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.858575231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.898167602] [controller_manager]: Overrun might occur, Total time : 2125.560 us (Expected < 2000.000 us) --> Read time : 15.640 us, Update time : 2108.277 us, Write time : 1.643 us (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409332.855033873] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409332.855110619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409333.855163547] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409333.855239060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.644221367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.243630 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409334.856865088] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.856899333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409335.855073861] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.855123716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409336.854994002] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409336.855044108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409337.854917940] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409337.854990809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409338.854997609] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409338.855049517] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409339.854978444] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.855031655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409340.854946146] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409340.854991112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409341.855069902] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409341.855133613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.855030043] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.855088283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [1778409265.263090998] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] [INFO] [1778409274.713339023] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:35:43 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:30 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] * Running on http://127.0.0.1:60145 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] * Running on http://172.30.43.138:60145 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:31] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.402644087] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.402700184] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.402711756] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.409908716] [moveit_2533370578.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.412109117] [moveit_2533370578.moveit.ros.rdf_loader]: Loaded robot model in 0.00203912 seconds (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.412141308] [moveit_2533370578.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.412151637] [moveit_2533370578.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [WARN] [1778409274.425153708] [moveit_2533370578.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.427943008] [moveit_2533370578.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.482222837] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.482347303] [moveit_2533370578.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483198170] [moveit_2533370578.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483662683] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483678102] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.483788482] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484138427] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484197670] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484879561] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.484894129] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.485428534] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.485933013] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [WARN] [1778409274.486267238] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [ERROR] [1778409274.486289971] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.996951404] [moveit_2533370578.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.997829577] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999298904] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999314924] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999567334] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999579918] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999601980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999606539] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409274.999617009] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.000486571] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.002854901] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.002868197] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.005024223] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.005039031] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.005709815] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.031011302] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.034492758] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.034671978] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.034697547] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409275.035510742] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:36] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  Generated 2 grasp options for object Box1. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:37 INFO  IK accepted: 2 grasp options. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019170695] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019279131] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019300111] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019324658] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [WARN] [1778409279.019427403] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.019959023] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.020076476] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.046283462] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.046829069] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.047552239] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Executing plan (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048809317] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048833904] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048852910] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048866256] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.048898347] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050387992] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050470579] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050505686] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.050666902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.051611853] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409279.051630799] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409280.352492091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409280.358335540] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:55 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:34:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139627241] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139710138] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139722472] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.139735567] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [WARN] [1778409312.139927632] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.140204899] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.140345817] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.157245027] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.157400303] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.157844907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Executing plan (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159879733] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159899421] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159913828] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159921002] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.159937012] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160386456] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160419599] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160441090] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.160580445] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.161194666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.161207751] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.761542234] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409312.766341108] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:27 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:43 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:43] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409343.176302110] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:58 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:58 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] 2026-05-10 12:35:58 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:36:01 [ ERROR] [INFO] [1778409358.313757967] [moveit_2533370578.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -__________________ test_pick_and_place_box_by_position[ur5e] ___________________ - -start_processes = Urls(ros_domain_id='14', robot_url='http://0.0.0.0:60145', storage_url='http://127.0.0.1:43829') - - @pytest.mark.timeout(321) - def test_pick_and_place_box_by_position(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Box("Box1", 0.2, 0.2, 0.2) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Box1. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-22-272481-DESKTOP-HG3Q5KV-5908 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [5912] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [5913] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [5914] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [5915] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [5916] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409262.549551434] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.550457761] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.553354602] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.555612692] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.555644413] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.555651045] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409262.555761630] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.729985698] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.732621687] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.732998733] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.733066853] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.741374118] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742687063] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742721548] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742745754] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742751355] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742805768] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409262.742844001] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409262.803210390] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.055317185] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.055374063] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.058681608] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.080953021] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.081839772] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.081992482] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.082034442] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.082059630] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.086465710] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.093388224] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.093430134] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.094539742] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.104989910] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.105207317] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.105966290] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.107140651] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.107177591] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.107740130] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.119096047] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.119339585] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.119954905] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.121406112] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.121440568] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.121960365] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.133002356] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.133243524] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.133744436] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.134985885] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.135016714] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.138622150] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.151262243] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.151503941] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.159180969] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.159218540] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.159600185] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.171009680] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.171267054] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.177676538] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.177711404] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.178019029] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.191023910] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.191270394] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.193976871] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.194014963] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.194618209] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409263.206927329] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.207167320] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.308475819] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 5916] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.560625400] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.560675385] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.561966084] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.575637796] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.576889119] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.576951507] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.581845691] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.582339024] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.585310080] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.586413045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.586448994] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.586850907] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.600894532] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.601178251] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.602805127] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409263.612952985] [controller_manager]: Overrun might occur, Total time : 8898.944 us (Expected < 2000.000 us) --> Read time : 11.702 us, Update time : 8884.737 us (Switch time : 8840.524 us (Switch chained mode time : 0.441 us, perform mode change time : 9.799 us, Activation time : 8813.131 us, Deactivation time : 0.140 us)), Write time : 2.505 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409263.613015333] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.037376 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.612969140] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.615170858] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.616300826] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.616341373] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.616779290] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.628921080] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.631006789] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.631341751] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.631401394] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.634990744] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.636187353] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.639094170] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.640163719] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.640199907] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.641309345] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.657238744] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.657506522] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.659464733] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.660829135] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.662209137] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.665137747] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.666263536] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.666306458] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.666634035] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.680954336] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.681254321] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.684966910] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.686215437] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.689020031] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.690004677] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.690041317] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.690488281] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_utmr5n4m -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.703309739] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.717557506] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718113307] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718266868] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718282738] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.718289942] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.719909455] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.726856455] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409263.728363854] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409263.731018569] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 5915] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409265.094024084] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 5913] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409265.387438825] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409265.387482358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409267.302218411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.240925 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409268.521119103] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.141647 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409270.655726791] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.749215 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409271.917770519] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409271.917824671] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409272.238216327] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.238751 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.854995613] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409272.855053884] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.855185279] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409273.855238721] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409274.855103029] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409274.855145400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409275.855039112] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409275.855091342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409276.855195118] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409276.855249081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.018341836] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 12.364320 ms (missed cycles : 7). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.060080729] [controller_manager]: Overrun might occur, Total time : 2015.961 us (Expected < 2000.000 us) --> Read time : 1952.540 us, Update time : 61.668 us, Write time : 1.753 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409277.855301274] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.855699701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409277.892541089] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.225895372] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.917836 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.444744492] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409278.855168922] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.855225119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409279.008537622] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.008582166] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.051281405] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.051343844] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409279.855302616] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409279.855345318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409280.124092009] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.114303 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409280.316084873] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409280.855014189] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409280.855058113] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.126164413] [controller_manager]: Overrun might occur, Total time : 2128.183 us (Expected < 2000.000 us) --> Read time : 14.557 us, Update time : 2111.993 us, Write time : 1.633 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.126206313] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.228766 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.855296851] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.855377734] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.855067697] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.855105298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.855059245] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409283.855114179] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409284.854982896] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409284.855040505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409285.855018714] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409285.855073298] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.139390759] [controller_manager]: Overrun might occur, Total time : 3316.512 us (Expected < 2000.000 us) --> Read time : 3181.095 us, Update time : 133.043 us, Write time : 2.374 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.139435113] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.457296 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409286.855389070] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.855429177] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.678614592] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.636926 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409287.855142507] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.855653993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.856549159] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.856596519] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.386304226] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.326620 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409289.855153616] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.855191267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.450298411] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.320805 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409290.854903404] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.854956374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.474354175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.376739 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409291.854858784] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.854933897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.765276491] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.298504 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.772776262] [controller_manager]: Overrun might occur, Total time : 2990.028 us (Expected < 2000.000 us) --> Read time : 28.615 us, Update time : 2958.677 us, Write time : 2.736 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409292.854952221] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.855007917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409293.855045529] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.855097668] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.906289378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.311942 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.962254353] [controller_manager]: Overrun might occur, Total time : 2172.272 us (Expected < 2000.000 us) --> Read time : 20.940 us, Update time : 2148.837 us, Write time : 2.495 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.854928226] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.854985916] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.854884480] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.854938803] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409296.854941558] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.854985982] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409297.854830332] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409297.854896297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.217584925] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.607228 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409298.854972974] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.855014423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409299.854979937] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409299.855036505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409300.854935218] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.854998027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409301.855057655] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409301.855111458] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409302.854942451] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409302.854998348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409303.855037568] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409303.855097532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409304.854979272] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409304.855039496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409305.855825209] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.855870064] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409306.854886888] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409306.854939358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409307.854910257] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409307.854979268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409308.854918743] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409308.854973277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409309.854883406] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409309.854941236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409310.462073398] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409310.854897887] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409310.854945647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.013138704] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.586948576] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409311.587046582] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409311.854893328] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.854946479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.160861944] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.160926577] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.754086783] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.854867122] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409312.854908330] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409313.854914692] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409313.854968284] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409314.854924200] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409314.854977781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409315.855092087] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409315.855140990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409316.855040709] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409316.855096445] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409317.855064449] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409317.855114585] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409318.855057005] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409318.855111879] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409319.855238653] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409319.855305129] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409320.854835531] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409320.854886108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.855049995] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409321.855124316] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409322.855096098] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409322.855149469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409323.855141204] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409323.855192321] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.738210221] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.232475 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.804632913] [controller_manager]: Overrun might occur, Total time : 2596.895 us (Expected < 2000.000 us) --> Read time : 13.135 us, Update time : 2582.237 us, Write time : 1.523 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409324.854939402] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.854985470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.855082794] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409325.855139452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.855056084] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409326.855109715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.854848193] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.854900312] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409328.855001595] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409328.855048754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409329.855052657] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409329.855100838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.678223647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.246081 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409330.855031462] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.855082169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.858219359] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.241863 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409331.858556786] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.858575231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.898167602] [controller_manager]: Overrun might occur, Total time : 2125.560 us (Expected < 2000.000 us) --> Read time : 15.640 us, Update time : 2108.277 us, Write time : 1.643 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409332.855033873] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409332.855110619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409333.855163547] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409333.855239060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.644221367] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.243630 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409334.856865088] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.856899333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409335.855073861] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.855123716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409336.854994002] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409336.855044108] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409337.854917940] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409337.854990809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409338.854997609] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409338.855049517] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409339.854978444] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.855031655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409340.854946146] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409340.854991112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409341.855069902] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409341.855133613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.855030043] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.855088283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409265.263090998] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.713339023] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:30 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:60145 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:60145 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:31] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.402644087] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.402700184] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.402711756] [moveit_2533370578.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.409908716] [moveit_2533370578.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.412109117] [moveit_2533370578.moveit.ros.rdf_loader]: Loaded robot model in 0.00203912 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.412141308] [moveit_2533370578.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.412151637] [moveit_2533370578.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409274.425153708] [moveit_2533370578.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.427943008] [moveit_2533370578.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.482222837] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.482347303] [moveit_2533370578.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483198170] [moveit_2533370578.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483662683] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483678102] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.483788482] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484138427] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484197670] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484879561] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.484894129] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.485428534] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.485933013] [moveit_2533370578.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409274.486267238] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409274.486289971] [moveit_2533370578.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.996951404] [moveit_2533370578.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.997829577] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999298904] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999314924] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999567334] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999579918] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999601980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999606539] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.999617009] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.000486571] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.002854901] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.002868197] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.005024223] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.005039031] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.005709815] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.031011302] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.034492758] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.034671978] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.034697547] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409275.035510742] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:36] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:36] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  Generated 2 grasp options for object Box1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:37 INFO  IK accepted: 2 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019170695] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019279131] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019300111] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019324658] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409279.019427403] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.019959023] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.020076476] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.046283462] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.046829069] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.047552239] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048809317] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048833904] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048852910] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048866256] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.048898347] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050387992] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050470579] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050505686] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.050666902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.051611853] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409279.051630799] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409280.352492091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409280.358335540] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139627241] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139710138] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139722472] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.139735567] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409312.139927632] [moveit_2533370578.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.140204899] [moveit_2533370578.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.140345817] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.157245027] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.157400303] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.157844907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159879733] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159899421] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159913828] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159921002] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.159937012] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160386456] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160419599] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160441090] [moveit_2533370578.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.160580445] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.161194666] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.161207751] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.761542234] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409312.766341108] [moveit_2533370578.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:43] "PUT /graspable/pick_up_object_by_position HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.176302110] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.313757967] [moveit_2533370578.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_box_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py::test_pick_and_place_box_by_position[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... -=================== 1 failed, 1 warning in 99.28s (0:01:39) ==================== - - -12:36:05.51 [INFO] Long running tasks: - 94.88s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests - 207.31s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:36:11.50 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests -12:36:11.51 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-FvhLSf -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py::test_pick_and_place_box_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:35:52 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-31-963480-DESKTOP-HG3Q5KV-6265 (testutils.py:33) - -2026-05-10 12:35:52 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [6281] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [6282] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [6283] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-4]: process started with pid [6284] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-5]: process started with pid [6285] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.329925081] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409272.331945475] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.333641632] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.337053290] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.337077747] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.337083428] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409272.337193001] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.343995090] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.346450863] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.346776472] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.346838820] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.353399575] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.354941761] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.354976227] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355004410] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355010562] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355080435] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.355129328] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.626530165] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.878841549] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.878904669] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.881939675] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.900703179] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901601741] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901858910] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901925636] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.901955643] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.906793816] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.912640647] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.912685732] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.913635978] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.924871857] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.925162501] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.925838705] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.928734081] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.928769008] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.929491025] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.940629284] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.940929920] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.941450313] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.942976909] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.943030631] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.943711665] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.954907434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.955230844] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.955807850] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.959315705] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.959358546] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.963699521] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409272.976727820] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.977199813] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.987156712] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.987204092] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409272.987516896] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409273.000948288] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.001255181] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.007310707] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.007353098] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.007646786] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409273.020946116] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.021266675] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.025441528] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.025491613] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.026093101] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-5] [INFO] [1778409273.038553997] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.038858491] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.126696651] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 6285] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.379275769] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.379330122] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.380690707] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.394723787] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.395845954] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.395933761] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.401195515] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.401722345] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.404717547] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.405672375] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.405711700] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.406119194] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.420620754] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.420921836] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.422147134] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409273.431053132] [controller_manager]: Overrun might occur, Total time : 7525.935 us (Expected < 2000.000 us) --> Read time : 12.804 us, Update time : 7510.366 us (Switch time : 7462.586 us (Switch chained mode time : 0.391 us, perform mode change time : 9.448 us, Activation time : 7436.325 us, Deactivation time : 0.151 us)), Write time : 2.765 us (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409273.431121281] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.703969 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.431064904] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.432907065] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.433971498] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.434007326] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.434254015] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.447319872] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.448813074] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.449144188] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.449196708] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.452251483] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.453674056] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.456849459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.458025634] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.458067113] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.459388975] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.474951665] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.475328386] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.477494728] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.480435371] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.481696237] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.485038552] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.486608816] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.486658070] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.487013966] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.502579623] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.502923326] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.506399119] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.507632233] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.510655883] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.511711604] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.511747753] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.512122866] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.525038979] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.536880769] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537237812] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537363030] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537374592] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.537381324] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.538954524] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.547717656] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409273.549792107] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [spawner-4] [INFO] [1778409273.552752131] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 6284] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [robot_state_publisher-2] [INFO] [1778409274.713334240] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6282] (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409275.017849728] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409275.017907949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409277.014267187] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.850136 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409278.226037963] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.621102 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409280.135848182] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431030 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409281.456654393] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.456706021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409281.700931158] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.514217 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409282.458205319] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.458232781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409282.978260447] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.843536 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409283.456894199] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409283.456943392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409284.456937511] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409284.456995892] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409285.456809418] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409285.456865655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409286.457407934] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.457456346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409286.494638366] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.221635 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409287.456878248] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.456934024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409287.554664337] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.678258065] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.841174 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409287.804411943] [controller_manager]: Overrun might occur, Total time : 2932.872 us (Expected < 2000.000 us) --> Read time : 16.721 us, Update time : 2914.288 us, Write time : 1.863 us (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.105762133] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.458989723] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.459043976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409288.705270125] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.705328696] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.762975467] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409288.763041383] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409289.456909119] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409289.456984462] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409290.011561700] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.222293757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.876945 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409290.459218180] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409290.459250652] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409291.457301783] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.457354864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409291.469718441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.301680 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409292.456942239] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.456988617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.750844168] [controller_manager]: Overrun might occur, Total time : 7345.348 us (Expected < 2000.000 us) --> Read time : 19.958 us, Update time : 7321.342 us, Write time : 4.048 us (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409292.750920232] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.503010 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409293.456899799] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.456961887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409293.906503706] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.086995 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409294.456823550] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409294.456896378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409295.456883594] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409295.456931315] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409296.456853328] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409296.456894546] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409297.456814830] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409297.456854906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.189558291] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141500 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409298.456792497] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409298.456869654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409299.456785134] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409299.456830751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409300.456745295] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409300.456796742] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409301.456772065] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409301.456828482] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409302.456927025] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409302.456983502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409303.456880051] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409303.456930537] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409304.456874385] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409304.456935851] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409305.456867453] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409305.456917578] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409306.456710354] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409306.456761502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409307.456807759] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409307.456848367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409308.456821706] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409308.456878304] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409309.456806281] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409309.456860815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409310.456846162] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409310.456910233] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409311.456774050] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409311.456822341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409312.456710107] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409312.456761494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409313.456929463] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409313.456985369] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409314.456781815] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409314.456836720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409315.456863355] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409315.456937877] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409316.456894444] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409316.456948477] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409317.456721782] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409317.456759183] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409318.456889265] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409318.456948357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409319.456867453] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409319.456928449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409320.062350915] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409320.456752982] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409320.456811363] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409320.613377322] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.456923827] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409321.456976617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409321.684114166] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.684174421] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.720373618] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409321.720439263] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409322.329573081] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409322.456987543] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409322.457062946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409323.456906410] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409323.456961234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409324.456881137] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.456929940] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.812052586] [controller_manager]: Overrun might occur, Total time : 2394.150 us (Expected < 2000.000 us) --> Read time : 39.095 us, Update time : 2353.372 us, Write time : 1.683 us (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409324.812083995] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.667335 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.456770823] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409325.456820367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.456799753] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409326.456856471] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.456752213] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.456799754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409328.456712729] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409328.456755891] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409329.456643592] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409329.456699288] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409330.456807654] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.456861516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.678223085] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.806454 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409331.456796058] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.456844550] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.854231276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.814545 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409332.456737090] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409332.456788347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409333.456848026] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409333.456901898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409334.456713498] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.456765557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.644356418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.939747 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409335.458709028] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.458746379] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.514190965] [controller_manager]: Overrun might occur, Total time : 2468.540 us (Expected < 2000.000 us) --> Read time : 13.917 us, Update time : 2452.950 us, Write time : 1.673 us (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409336.456834818] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409336.456887839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409337.456820339] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409337.456874702] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409338.456797879] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409338.456867762] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409339.456795070] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.456845236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409340.456747072] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409340.456795705] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409341.456838050] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409341.456909075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.456828362] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.456876663] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.456679776] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409343.456729300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409344.456726168] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409344.456777075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.456877551] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409345.456930442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409346.456721266] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409346.456791680] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409347.456743598] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409347.456795496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409348.456697075] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409348.456746730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409349.456648931] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409349.456693606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409350.456673349] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409350.456721390] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409351.456703350] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409351.456752333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [INFO] [1778409352.456646819] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [ros2_control_node-1] [WARN] [1778409352.456696313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [1778409274.904707518] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] [INFO] [1778409284.188595033] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:35:52 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] * Running on http://127.0.0.1:33273 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] * Running on http://172.30.43.138:33273 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:40] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:44 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.015243124] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.015287419] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.015297297] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.022040389] [moveit_1358702927.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.024506086] [moveit_1358702927.moveit.ros.rdf_loader]: Loaded robot model in 0.00230985 seconds (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.024536974] [moveit_1358702927.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.024546983] [moveit_1358702927.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [WARN] [1778409285.036991738] [moveit_1358702927.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.040317718] [moveit_1358702927.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.107739013] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.107916620] [moveit_1358702927.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.108931799] [moveit_1358702927.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.109503946] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.109520237] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.109653711] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110098927] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110178598] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110894278] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.110910208] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.111408254] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409285.111891743] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [WARN] [1778409285.112150485] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [ERROR] [1778409285.112166646] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.143058463] [moveit_1358702927.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.143697537] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.144955147] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.144970366] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145264364] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145277239] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145298820] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145302727] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.145311414] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.146294992] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.148497537] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.148511363] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.150697727] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.150714098] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.151371919] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.177133053] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.180569552] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.180794159] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.180825669] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409286.181678219] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:46] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  Generated 2 grasp options for object Box1. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:47 INFO  IK accepted: 2 grasp options. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733000647] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733119372] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733137447] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733155411] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [WARN] [1778409288.733221236] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733715816] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.733866151] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.752424549] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.752860999] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.753311334] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:48 INFO  Executing plan (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759531328] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759571495] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759601572] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759613584] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.759639564] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.761896362] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.761963539] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.761991372] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.762129194] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.763878604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409288.763898422] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409290.014115276] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409290.019725943] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:34:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:05 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698886720] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698960219] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698971431] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.698983223] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [WARN] [1778409321.699120544] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.699372122] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.699416577] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.717973357] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.718168809] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.718593285] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:21 INFO  Executing plan (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719126153] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719149427] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719165317] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719172511] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719189303] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719844547] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719879364] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.719902668] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.720052783] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.720686888] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409321.720700604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409322.371186893] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409322.375782694] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:37 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:35:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:52] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409352.952259602] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:36:07 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:36:07 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] 2026-05-10 12:36:08 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:36:10 [ ERROR] [INFO] [1778409368.079316218] [moveit_1358702927.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________ test_pick_and_place_box_by_id[ur5e] ______________________ - -start_processes = Urls(ros_domain_id='214', robot_url='http://0.0.0.0:33273', storage_url='http://127.0.0.1:35055') - - @pytest.mark.timeout(400) - def test_pick_and_place_box_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Box("Box1", 0.2, 0.2, 0.2) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Box1. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-34-31-963480-DESKTOP-HG3Q5KV-6265 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [6281] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [6282] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [6283] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [6284] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [6285] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.329925081] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409272.331945475] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.333641632] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.337053290] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.337077747] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.337083428] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409272.337193001] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.343995090] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.346450863] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.346776472] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.346838820] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.353399575] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.354941761] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.354976227] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355004410] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355010562] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355080435] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.355129328] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.626530165] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.878841549] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.878904669] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.881939675] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.900703179] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901601741] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901858910] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901925636] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.901955643] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.906793816] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.912640647] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.912685732] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.913635978] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.924871857] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.925162501] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.925838705] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.928734081] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.928769008] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.929491025] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.940629284] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.940929920] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.941450313] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.942976909] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.943030631] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.943711665] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.954907434] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.955230844] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.955807850] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.959315705] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.959358546] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.963699521] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409272.976727820] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.977199813] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.987156712] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.987204092] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409272.987516896] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409273.000948288] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.001255181] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.007310707] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.007353098] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.007646786] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409273.020946116] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.021266675] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.025441528] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.025491613] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.026093101] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409273.038553997] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.038858491] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.126696651] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 6285] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.379275769] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.379330122] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.380690707] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.394723787] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.395845954] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.395933761] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.401195515] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.401722345] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.404717547] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.405672375] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.405711700] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.406119194] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.420620754] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.420921836] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.422147134] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409273.431053132] [controller_manager]: Overrun might occur, Total time : 7525.935 us (Expected < 2000.000 us) --> Read time : 12.804 us, Update time : 7510.366 us (Switch time : 7462.586 us (Switch chained mode time : 0.391 us, perform mode change time : 9.448 us, Activation time : 7436.325 us, Deactivation time : 0.151 us)), Write time : 2.765 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409273.431121281] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.703969 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.431064904] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.432907065] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.433971498] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.434007326] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.434254015] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.447319872] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.448813074] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.449144188] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.449196708] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.452251483] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.453674056] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.456849459] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.458025634] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.458067113] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.459388975] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.474951665] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.475328386] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.477494728] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.480435371] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.481696237] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.485038552] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.486608816] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.486658070] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.487013966] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.502579623] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.502923326] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.506399119] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.507632233] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.510655883] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.511711604] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.511747753] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.512122866] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_ezvowfjz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.525038979] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.536880769] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537237812] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537363030] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537374592] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.537381324] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.538954524] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.547717656] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409273.549792107] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409273.552752131] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 6284] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409274.713334240] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 6282] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409275.017849728] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409275.017907949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409277.014267187] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.850136 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409278.226037963] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.621102 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409280.135848182] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431030 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409281.456654393] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.456706021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409281.700931158] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.514217 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409282.458205319] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.458232781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409282.978260447] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.843536 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409283.456894199] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409283.456943392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409284.456937511] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409284.456995892] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409285.456809418] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409285.456865655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409286.457407934] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.457456346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409286.494638366] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 11.221635 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409287.456878248] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.456934024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409287.554664337] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.678258065] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.841174 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409287.804411943] [controller_manager]: Overrun might occur, Total time : 2932.872 us (Expected < 2000.000 us) --> Read time : 16.721 us, Update time : 2914.288 us, Write time : 1.863 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.105762133] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.458989723] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.459043976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409288.705270125] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.705328696] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.762975467] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409288.763041383] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409289.456909119] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409289.456984462] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409290.011561700] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.222293757] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.876945 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409290.459218180] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409290.459250652] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409291.457301783] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.457354864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409291.469718441] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.301680 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409292.456942239] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.456988617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.750844168] [controller_manager]: Overrun might occur, Total time : 7345.348 us (Expected < 2000.000 us) --> Read time : 19.958 us, Update time : 7321.342 us, Write time : 4.048 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409292.750920232] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 7.503010 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409293.456899799] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.456961887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409293.906503706] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.086995 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409294.456823550] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409294.456896378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409295.456883594] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409295.456931315] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409296.456853328] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409296.456894546] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409297.456814830] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409297.456854906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.189558291] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141500 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409298.456792497] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409298.456869654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409299.456785134] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409299.456830751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409300.456745295] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409300.456796742] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409301.456772065] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409301.456828482] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409302.456927025] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409302.456983502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409303.456880051] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409303.456930537] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409304.456874385] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409304.456935851] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409305.456867453] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409305.456917578] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409306.456710354] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409306.456761502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409307.456807759] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409307.456848367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409308.456821706] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409308.456878304] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409309.456806281] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409309.456860815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409310.456846162] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409310.456910233] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409311.456774050] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409311.456822341] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409312.456710107] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409312.456761494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409313.456929463] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409313.456985369] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409314.456781815] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409314.456836720] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409315.456863355] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409315.456937877] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409316.456894444] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409316.456948477] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409317.456721782] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409317.456759183] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409318.456889265] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409318.456948357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409319.456867453] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409319.456928449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409320.062350915] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409320.456752982] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409320.456811363] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409320.613377322] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.456923827] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409321.456976617] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409321.684114166] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.684174421] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.720373618] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409321.720439263] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409322.329573081] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409322.456987543] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409322.457062946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409323.456906410] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409323.456961234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409324.456881137] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.456929940] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.812052586] [controller_manager]: Overrun might occur, Total time : 2394.150 us (Expected < 2000.000 us) --> Read time : 39.095 us, Update time : 2353.372 us, Write time : 1.683 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409324.812083995] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.667335 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.456770823] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409325.456820367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.456799753] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409326.456856471] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.456752213] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.456799754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409328.456712729] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409328.456755891] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409329.456643592] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409329.456699288] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409330.456807654] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.456861516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.678223085] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.806454 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409331.456796058] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.456844550] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.854231276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.814545 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409332.456737090] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409332.456788347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409333.456848026] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409333.456901898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409334.456713498] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.456765557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.644356418] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.939747 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409335.458709028] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.458746379] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.514190965] [controller_manager]: Overrun might occur, Total time : 2468.540 us (Expected < 2000.000 us) --> Read time : 13.917 us, Update time : 2452.950 us, Write time : 1.673 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409336.456834818] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409336.456887839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409337.456820339] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409337.456874702] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409338.456797879] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409338.456867762] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409339.456795070] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.456845236] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409340.456747072] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409340.456795705] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409341.456838050] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409341.456909075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.456828362] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.456876663] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.456679776] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409343.456729300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409344.456726168] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409344.456777075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.456877551] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409345.456930442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409346.456721266] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409346.456791680] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409347.456743598] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409347.456795496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409348.456697075] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409348.456746730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409349.456648931] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409349.456693606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409350.456673349] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409350.456721390] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409351.456703350] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409351.456752333] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409352.456646819] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409352.456696313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409274.904707518] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409284.188595033] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:40 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:33273 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:33273 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:40] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.015243124] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.015287419] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.015297297] [moveit_1358702927.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.022040389] [moveit_1358702927.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.024506086] [moveit_1358702927.moveit.ros.rdf_loader]: Loaded robot model in 0.00230985 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.024536974] [moveit_1358702927.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.024546983] [moveit_1358702927.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409285.036991738] [moveit_1358702927.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.040317718] [moveit_1358702927.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.107739013] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.107916620] [moveit_1358702927.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.108931799] [moveit_1358702927.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.109503946] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.109520237] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.109653711] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110098927] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110178598] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110894278] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.110910208] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.111408254] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409285.111891743] [moveit_1358702927.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409285.112150485] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409285.112166646] [moveit_1358702927.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.143058463] [moveit_1358702927.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.143697537] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.144955147] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.144970366] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145264364] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145277239] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145298820] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145302727] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.145311414] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.146294992] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.148497537] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.148511363] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.150697727] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.150714098] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.151371919] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.177133053] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.180569552] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.180794159] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.180825669] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409286.181678219] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:46] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:34:46] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  Generated 2 grasp options for object Box1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:47 INFO  IK accepted: 2 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733000647] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733119372] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733137447] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733155411] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409288.733221236] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733715816] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.733866151] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.752424549] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.752860999] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.753311334] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:48 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759531328] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759571495] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759601572] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759613584] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.759639564] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.761896362] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.761963539] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.761991372] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.762129194] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.763878604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409288.763898422] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409290.014115276] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409290.019725943] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:34:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698886720] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698960219] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698971431] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.698983223] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409321.699120544] [moveit_1358702927.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.699372122] [moveit_1358702927.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.699416577] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.717973357] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.718168809] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.718593285] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:21 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719126153] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719149427] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719165317] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719172511] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719189303] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719844547] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719879364] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.719902668] [moveit_1358702927.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.720052783] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.720686888] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409321.720700604] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409322.371186893] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409322.375782694] [moveit_1358702927.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:52] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409352.952259602] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:07 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:07 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:08 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409368.079316218] [moveit_1358702927.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_box_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py::test_pick_and_place_box_by_id[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... -=================== 1 failed, 1 warning in 99.71s (0:01:39) ==================== - - -12:36:35.57 [INFO] Long running tasks: - 71.05s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 237.37s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:36:35.69 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests -12:36:35.69 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-7OBLoJ -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py::test_pick_up_box_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:36:17 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-02-437060-DESKTOP-HG3Q5KV-7847 (testutils.py:33) - -2026-05-10 12:36:17 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [7851] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [7852] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [7853] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-4]: process started with pid [7854] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-5]: process started with pid [7855] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409362.698293946] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.701067742] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.703443301] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.706279775] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.706312116] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.706318499] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409362.706422085] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.714017898] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.716407630] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.716780859] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.716849529] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.724328220] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725375660] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725403473] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725426928] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725431446] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725478085] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.725509765] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409362.947512446] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.199660102] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.199713924] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.202547148] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.223923139] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.224956984] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.225118901] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.225155120] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.225177673] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.229565932] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.235852150] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.235899280] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.236987960] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.247926694] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.248157157] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.248964270] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.251922977] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.251970016] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.252611225] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.263533594] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.263754343] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.264252299] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.265892857] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.265927993] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.266545006] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.277858744] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.278077075] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.278662437] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.280004481] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.280043625] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.284537915] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.297601527] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.297842615] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.306181274] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.306223264] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.306546919] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.319885525] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.320108764] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.326020832] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.326056981] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.326399803] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.339696263] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.339918735] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.340517734] [freedrive_mode_controller]: Freedrive mode has been enabled successfully. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.342283970] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.342312734] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.342746799] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-5] [INFO] [1778409363.355878766] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.356165686] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.453364310] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 7855] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.705919862] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.705981198] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.707495821] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.721981680] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.723162905] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.723228559] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.728226161] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.728986991] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.731738520] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.732814379] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.732857201] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.733206363] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.747728913] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.748057638] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.749629644] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409363.760097129] [controller_manager]: Overrun might occur, Total time : 9329.893 us (Expected < 2000.000 us) --> Read time : 13.095 us, Update time : 9313.642 us (Switch time : 9266.693 us (Switch chained mode time : 0.390 us, perform mode change time : 12.183 us, Activation time : 9238.179 us, Deactivation time : 0.141 us)), Write time : 3.156 us (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409363.760182657] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.516241 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.760105751] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.761810936] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.762684896] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.762724191] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.763112749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.775962826] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.777994653] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.778305427] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.778370631] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.781514791] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.782878346] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.785912918] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.786971243] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.787006510] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.788137395] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.803740166] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.804125614] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.806453497] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.807545877] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.808855481] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.811766713] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.812940608] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.812975554] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.813348814] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.827875823] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.828305714] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.831666370] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.832879039] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.835639034] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.836887059] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.836929179] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.837256732] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.850284111] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.866050885] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866580216] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866707888] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866726684] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.866737024] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.868270081] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.875743106] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.877122367] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [spawner-4] [INFO] [1778409363.879897589] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 7854] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409365.249884154] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7852] (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409365.541017098] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409365.541067364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.477540211] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.873825 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.726213048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.547083 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.850926975] [controller_manager]: Overrun might occur, Total time : 2078.966 us (Expected < 2000.000 us) --> Read time : 176.991 us, Update time : 1900.301 us, Write time : 1.674 us (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.519038159] [controller_manager]: Overrun might occur, Total time : 2313.550 us (Expected < 2000.000 us) --> Read time : 138.918 us, Update time : 2172.007 us, Write time : 2.625 us (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.519103603] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.436716 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409372.089874163] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409372.089927534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.046941930] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.046994119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.046863138] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409374.046916770] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409375.046791576] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409375.046841932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409376.046838342] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409376.046883147] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409377.046990234] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.047059516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [1778409365.411366456] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] [INFO] [1778409375.566907179] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:36:17 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:10 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] * Running on http://127.0.0.1:47847 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] * Running on http://172.30.43.138:47847 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:11] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:15 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.499689743] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.499754015] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.499771038] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.505902168] [moveit_1079793702.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.508078764] [moveit_1079793702.moveit.ros.rdf_loader]: Loaded robot model in 0.00203154 seconds (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.508134931] [moveit_1079793702.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.508152684] [moveit_1079793702.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [WARN] [1778409375.519694890] [moveit_1079793702.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.522656955] [moveit_1079793702.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.580353904] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.580498830] [moveit_1079793702.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.581597327] [moveit_1079793702.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582155398] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582172861] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582278827] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582599016] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.582677956] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.583252162] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.583267541] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.583709040] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.584193540] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [WARN] [1778409375.584469935] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [ERROR] [1778409375.584490324] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.659926833] [moveit_1079793702.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.660599331] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.661708439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.661722726] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662062392] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662097438] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662126804] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662131573] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662152443] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.662841953] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.665617416] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.665631613] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.667703350] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.667721444] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.668501887] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.696451154] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.699843500] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.700016248] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.700042528] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409375.700677414] [moveit_1079793702.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:17] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:17] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409377.217789605] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:32 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:32 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] 2026-05-10 12:36:32 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:36:35 [ ERROR] [INFO] [1778409392.346505595] [moveit_1079793702.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_________________________ test_pick_up_box_by_id[ur5e] _________________________ - -start_processes = Urls(ros_domain_id='162', robot_url='http://0.0.0.0:47847', storage_url='http://127.0.0.1:55709') - - @pytest.mark.timeout(400) - def test_pick_up_box_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Box("Box1", 0.2, 0.2, 0.2) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:27: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Box1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-02-437060-DESKTOP-HG3Q5KV-7847 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [7851] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [7852] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [7853] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [7854] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [7855] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409362.698293946] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.701067742] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.703443301] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.706279775] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.706312116] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.706318499] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409362.706422085] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.714017898] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.716407630] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.716780859] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.716849529] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.724328220] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725375660] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725403473] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725426928] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725431446] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725478085] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.725509765] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409362.947512446] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.199660102] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.199713924] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.202547148] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.223923139] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.224956984] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.225118901] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.225155120] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.225177673] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.229565932] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.235852150] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.235899280] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.236987960] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.247926694] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.248157157] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.248964270] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.251922977] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.251970016] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.252611225] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.263533594] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.263754343] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.264252299] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.265892857] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.265927993] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.266545006] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.277858744] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.278077075] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.278662437] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.280004481] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.280043625] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.284537915] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.297601527] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.297842615] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.306181274] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.306223264] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.306546919] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.319885525] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.320108764] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.326020832] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.326056981] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.326399803] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.339696263] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.339918735] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.340517734] [freedrive_mode_controller]: Freedrive mode has been enabled successfully. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.342283970] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.342312734] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.342746799] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409363.355878766] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.356165686] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.453364310] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 7855] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.705919862] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.705981198] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.707495821] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.721981680] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.723162905] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.723228559] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.728226161] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.728986991] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.731738520] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.732814379] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.732857201] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.733206363] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.747728913] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.748057638] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.749629644] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409363.760097129] [controller_manager]: Overrun might occur, Total time : 9329.893 us (Expected < 2000.000 us) --> Read time : 13.095 us, Update time : 9313.642 us (Switch time : 9266.693 us (Switch chained mode time : 0.390 us, perform mode change time : 12.183 us, Activation time : 9238.179 us, Deactivation time : 0.141 us)), Write time : 3.156 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409363.760182657] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.516241 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.760105751] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.761810936] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.762684896] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.762724191] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.763112749] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.775962826] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.777994653] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.778305427] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.778370631] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.781514791] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.782878346] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.785912918] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.786971243] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.787006510] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.788137395] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.803740166] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.804125614] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.806453497] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.807545877] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.808855481] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.811766713] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.812940608] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.812975554] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.813348814] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.827875823] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.828305714] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.831666370] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.832879039] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.835639034] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.836887059] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.836929179] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.837256732] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6pzfxkdm -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.850284111] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.866050885] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866580216] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866707888] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866726684] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.866737024] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.868270081] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.875743106] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.877122367] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409363.879897589] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 7854] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409365.249884154] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7852] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409365.541017098] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409365.541067364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.477540211] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.873825 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.726213048] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.547083 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.850926975] [controller_manager]: Overrun might occur, Total time : 2078.966 us (Expected < 2000.000 us) --> Read time : 176.991 us, Update time : 1900.301 us, Write time : 1.674 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.519038159] [controller_manager]: Overrun might occur, Total time : 2313.550 us (Expected < 2000.000 us) --> Read time : 138.918 us, Update time : 2172.007 us, Write time : 2.625 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.519103603] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.436716 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409372.089874163] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409372.089927534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.046941930] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.046994119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.046863138] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409374.046916770] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409375.046791576] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409375.046841932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409376.046838342] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409376.046883147] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409377.046990234] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.047059516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409365.411366456] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.566907179] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:47847 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:47847 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:11] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.499689743] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.499754015] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.499771038] [moveit_1079793702.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.505902168] [moveit_1079793702.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.508078764] [moveit_1079793702.moveit.ros.rdf_loader]: Loaded robot model in 0.00203154 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.508134931] [moveit_1079793702.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.508152684] [moveit_1079793702.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409375.519694890] [moveit_1079793702.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.522656955] [moveit_1079793702.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.580353904] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.580498830] [moveit_1079793702.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.581597327] [moveit_1079793702.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582155398] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582172861] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582278827] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582599016] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.582677956] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.583252162] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.583267541] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.583709040] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.584193540] [moveit_1079793702.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409375.584469935] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409375.584490324] [moveit_1079793702.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.659926833] [moveit_1079793702.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.660599331] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.661708439] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.661722726] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662062392] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662097438] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662126804] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662131573] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662152443] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.662841953] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.665617416] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.665631613] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.667703350] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.667721444] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.668501887] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.696451154] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.699843500] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.700016248] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.700042528] [moveit_1079793702.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.700677414] [moveit_1079793702.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:17] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:17] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409377.217789605] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409392.346505595] [moveit_1079793702.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:13 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_box_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py::test_pick_up_box_by_id[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 33.36s ========================= - - -12:37:05.62 [INFO] Long running tasks: - 101.09s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 267.42s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:37:08.86 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests -12:37:08.86 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-cmgKAE -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py::test_pick_up_mesh_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:36:50 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-36-854431-DESKTOP-HG3Q5KV-8758 (testutils.py:33) - -2026-05-10 12:36:50 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [8762] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [8763] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [8764] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-4]: process started with pid [8765] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-5]: process started with pid [8766] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [robot_state_publisher-2] [INFO] [1778409397.162957392] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.168983002] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.171939720] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.174288158] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.174318756] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.174324337] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.174428915] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.343808993] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.346508607] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.346891495] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.346964684] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.355604571] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357018517] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357056870] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357081106] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357086326] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357149115] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.357187167] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.456317492] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.708408366] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.708467237] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.711002745] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.724061493] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.725217886] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.725358272] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.734219074] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.734951275] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.737781293] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.738611045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.738655970] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.743747895] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.755901879] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.756164708] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.757312880] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.768043784] [controller_manager]: Overrun might occur, Total time : 9306.634 us (Expected < 2000.000 us) --> Read time : 13.035 us, Update time : 9291.545 us (Switch time : 9249.525 us (Switch chained mode time : 0.411 us, perform mode change time : 10.840 us, Activation time : 9224.267 us, Deactivation time : 0.220 us)), Write time : 2.054 us (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.768101695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.447075 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.768058453] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.770165011] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.771071323] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.771161043] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.771552898] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.781835135] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.783901827] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.784134785] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.784171615] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.785560369] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.786844014] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.789675353] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.790627451] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.790660604] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.791993663] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.805917535] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.806209870] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.808192132] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.809531497] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.810869876] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.813793790] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.814699977] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.814729242] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.815009956] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.828079596] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.828333007] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.831595827] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.832868044] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.835897791] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.836877863] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.836919963] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.837206447] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.847869208] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.859939803] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860149882] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860321649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860345444] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.860369770] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.862742820] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.869799007] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.870988457] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-4] [INFO] [1778409397.873960789] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409397.960850175] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 8765] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.213386105] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.213434967] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.213795773] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.237905859] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239094683] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239234960] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239267021] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.239279785] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.240494183] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.246001036] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.246052414] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.247493256] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.261808677] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.262109004] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.263047837] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.266109619] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.266156588] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.267009896] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.281629349] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.281937509] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.282416009] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.284199087] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.284233482] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.284972141] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.299837702] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.300129847] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.300617524] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.304179016] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.304210977] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.304541575] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.319994914] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.320335231] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.330359668] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.330420333] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.330851753] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.345875269] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.346168286] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.352481447] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.352510853] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.352755147] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.367855264] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.368210830] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.372361136] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.372399128] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.372668310] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [spawner-5] [INFO] [1778409398.388003935] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.388301906] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 8766] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [robot_state_publisher-2] [INFO] [1778409399.647017065] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8763] (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409399.937946051] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409399.938000945] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.042242378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.588370 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.866249029] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.594821 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.092568163] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.092623027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.341779581] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.341841449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409408.342128646] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409408.342208658] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409409.341795488] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409409.341856474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.342059747] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [ros2_control_node-1] [WARN] [1778409410.342125331] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [1778409399.824482336] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] [INFO] [1778409410.431215087] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:36:50 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] * Running on http://127.0.0.1:58965 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] * Running on http://172.30.43.138:58965 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:45] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.493135262] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.493178655] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.493189475] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.500468376] [moveit_2109978589.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.503447206] [moveit_2109978589.moveit.ros.rdf_loader]: Loaded robot model in 0.00278104 seconds (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.503488414] [moveit_2109978589.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.503505937] [moveit_2109978589.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [WARN] [1778409409.517172621] [moveit_2109978589.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.520555374] [moveit_2109978589.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.583144240] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.583316337] [moveit_2109978589.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.584473575] [moveit_2109978589.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585171376] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585188408] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585313431] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585785317] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.585873335] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.586653617] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.586668736] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.587360320] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.588086701] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [WARN] [1778409409.588369849] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [ERROR] [1778409409.588386601] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.745309033] [moveit_2109978589.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.746133379] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747376030] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747393664] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747739020] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747753958] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747783745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747794606] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.747822128] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.748466172] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.750997893] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.751013703] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.753529182] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.753544511] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.754616788] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.788634115] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.793023476] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.793248463] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.793279582] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409409.794424843] [moveit_2109978589.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:50] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:50] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409410.563385788] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:37:05 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:37:05 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] 2026-05-10 12:37:05 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:37:08 [ ERROR] [INFO] [1778409425.712762606] [moveit_2109978589.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________ test_pick_up_mesh_by_position[ur5e] ______________________ - -start_processes = Urls(ros_domain_id='147', robot_url='http://0.0.0.0:58965', storage_url='http://127.0.0.1:33125') - - @pytest.mark.timeout(321) - def test_pick_up_mesh_by_position(start_processes: Urls) -> None: - assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" -  - storage_client.URL = start_processes.storage_url -  - storage_client.create_asset( - MESH_ASSET_ID, - MESH_PATH.read_bytes(), - description="Test mesh for UR5e collision avoidance.", - ) -  - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Mesh("Mesh1", MESH_ASSET_ID) -> scene_service.upsert_graspable(object.id, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:42: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2/exceptions/helpers.py:20: in wrapper - return func(*args, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^ -src/python/arcor2_scene_data/scene_service.py:150: in upsert_graspable - params, model_type = _collision_params(model, mesh_parameters) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -model = 'Mesh1', mesh_parameters = None - - def _collision_params(model: Models, mesh_parameters: None | MeshParameters = None) -> tuple[dict[str, Any], str]: -> params = model.to_dict() - ^^^^^^^^^^^^^ -E AttributeError: 'str' object has no attribute 'to_dict' - -src/python/arcor2_scene_data/scene_service.py:72: AttributeError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-36-854431-DESKTOP-HG3Q5KV-8758 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [8762] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [8763] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [8764] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [8765] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [8766] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409397.162957392] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.168983002] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.171939720] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.174288158] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.174318756] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.174324337] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.174428915] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.343808993] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.346508607] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.346891495] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.346964684] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.355604571] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357018517] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357056870] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357081106] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357086326] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357149115] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.357187167] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.456317492] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.708408366] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.708467237] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.711002745] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.724061493] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.725217886] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.725358272] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.734219074] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.734951275] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.737781293] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.738611045] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.738655970] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.743747895] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.755901879] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.756164708] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.757312880] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.768043784] [controller_manager]: Overrun might occur, Total time : 9306.634 us (Expected < 2000.000 us) --> Read time : 13.035 us, Update time : 9291.545 us (Switch time : 9249.525 us (Switch chained mode time : 0.411 us, perform mode change time : 10.840 us, Activation time : 9224.267 us, Deactivation time : 0.220 us)), Write time : 2.054 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.768101695] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.447075 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.768058453] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.770165011] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.771071323] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.771161043] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.771552898] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.781835135] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.783901827] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.784134785] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.784171615] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.785560369] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.786844014] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.789675353] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.790627451] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.790660604] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.791993663] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.805917535] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.806209870] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.808192132] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.809531497] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.810869876] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.813793790] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.814699977] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.814729242] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.815009956] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.828079596] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.828333007] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.831595827] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.832868044] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.835897791] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.836877863] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.836919963] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.837206447] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.847869208] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.859939803] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860149882] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860321649] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860345444] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.860369770] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.862742820] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.869799007] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.870988457] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409397.873960789] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409397.960850175] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 8765] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.213386105] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.213434967] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.213795773] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.237905859] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239094683] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239234960] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239267021] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.239279785] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.240494183] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.246001036] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.246052414] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.247493256] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.261808677] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.262109004] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.263047837] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.266109619] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.266156588] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.267009896] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.281629349] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.281937509] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.282416009] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.284199087] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.284233482] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.284972141] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.299837702] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.300129847] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.300617524] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.304179016] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.304210977] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.304541575] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.319994914] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.320335231] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.330359668] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.330420333] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.330851753] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.345875269] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.346168286] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.352481447] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.352510853] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.352755147] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.367855264] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.368210830] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.372361136] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.372399128] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.372668310] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_r5kpoupx -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409398.388003935] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.388301906] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 8766] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409399.647017065] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8763] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409399.937946051] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409399.938000945] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.042242378] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.588370 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.866249029] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.594821 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.092568163] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.092623027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.341779581] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.341841449] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409408.342128646] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409408.342208658] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409409.341795488] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409409.341856474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.342059747] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409410.342125331] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409399.824482336] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.431215087] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:44 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:58965 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:58965 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:45] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.493135262] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.493178655] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.493189475] [moveit_2109978589.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.500468376] [moveit_2109978589.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.503447206] [moveit_2109978589.moveit.ros.rdf_loader]: Loaded robot model in 0.00278104 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.503488414] [moveit_2109978589.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.503505937] [moveit_2109978589.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409409.517172621] [moveit_2109978589.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.520555374] [moveit_2109978589.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.583144240] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.583316337] [moveit_2109978589.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.584473575] [moveit_2109978589.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585171376] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585188408] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585313431] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585785317] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.585873335] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.586653617] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.586668736] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.587360320] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.588086701] [moveit_2109978589.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409409.588369849] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409409.588386601] [moveit_2109978589.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.745309033] [moveit_2109978589.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.746133379] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747376030] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747393664] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747739020] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747753958] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747783745] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747794606] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.747822128] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.748466172] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.750997893] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.751013703] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.753529182] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.753544511] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.754616788] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.788634115] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.793023476] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.793248463] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.793279582] [moveit_2109978589.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409409.794424843] [moveit_2109978589.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:50] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:50] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.563385788] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409425.712762606] [moveit_2109978589.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:18 - src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.test_pick_up.test_pick_up_mesh_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py::test_pick_up_mesh_by_position[ur5e] - AttributeError: 'str' object has no attribute 'to_dict' -======================== 1 failed, 1 warning in 32.20s ========================= - - -12:37:35.66 [INFO] Long running tasks: - 84.15s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - 131.14s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 297.46s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:37:58.25 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests -12:37:58.25 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-gbn5x0 -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py::test_pick_and_place_mesh_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:37:40 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-09-975076-DESKTOP-HG3Q5KV-9294 (testutils.py:33) - -2026-05-10 12:37:40 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [9301] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [9302] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [9303] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-4]: process started with pid [9304] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-5]: process started with pid [9305] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [robot_state_publisher-2] [INFO] [1778409430.283072484] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.290127024] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.293110835] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.295385617] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.295408190] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.295417127] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.295502138] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.460250108] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.463333817] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.463758784] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.463848415] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.473434458] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475031773] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475068273] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475098510] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475105183] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475187319] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.475240019] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.575491178] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.827882954] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.827937067] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.830258218] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.842762256] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.843890946] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.844004481] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.851256036] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.852075537] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.854937024] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.855728088] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.855761962] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.860790948] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.875133631] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.875395533] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.876392513] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.888327004] [controller_manager]: Overrun might occur, Total time : 10500.567 us (Expected < 2000.000 us) --> Read time : 12.654 us, Update time : 10485.718 us (Switch time : 10431.786 us (Switch chained mode time : 0.421 us, perform mode change time : 16.111 us, Activation time : 10400.527 us, Deactivation time : 0.320 us)), Write time : 2.195 us (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.888397017] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.681132 ms (missed cycles : 6). (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.888336377] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.890746205] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.891579003] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.891632404] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.892101676] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.902523645] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.904737415] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.905006041] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.905051728] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.906578349] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.907950646] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.910702891] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.911613246] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.911647551] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.912672869] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.926847733] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.927110793] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.929276157] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.930672471] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.931920958] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.934792514] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.935645219] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.935680917] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.935906376] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.948847494] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.949093797] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.952600080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.954013912] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.956894364] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.958003287] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.958038324] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.958328104] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.969527479] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.981286372] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981577581] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981811977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981868745] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.981971560] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.984779390] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.992642003] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.994081759] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-4] [INFO] [1778409430.997018094] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.083562408] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 9304] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.336052076] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.336110427] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.336471422] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.361236664] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362605320] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362759884] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362797024] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.362810289] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.364030217] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.371276932] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.371317229] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.372861593] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.389025791] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.389365848] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.390468733] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.393397488] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.393434449] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.394269941] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.411003271] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.411368926] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.412190507] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.415473695] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.415513420] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.416306347] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.433069809] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.433410166] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.434035424] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.437238225] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.437270206] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.437555177] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.452842175] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.453169652] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.463384456] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.463447686] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.463758938] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.478987382] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.479295043] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.485501135] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.485546211] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.485877835] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.500733118] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.501059497] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.505489947] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.505525755] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.505784477] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [spawner-5] [INFO] [1778409431.520771271] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.521083360] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 9305] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [robot_state_publisher-2] [INFO] [1778409432.763700913] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9302] (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.073391159] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409433.073445442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.992241141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.526579 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.161177547] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.462914 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.246199523] [controller_manager]: Overrun might occur, Total time : 2426.716 us (Expected < 2000.000 us) --> Read time : 12.864 us, Update time : 2412.239 us, Write time : 1.613 us (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.006216951] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.502268 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409439.434062068] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.434115389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409440.434129650] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409440.434239659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409441.434104073] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409441.434166212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409442.434163204] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409442.434230552] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409443.434155498] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409443.434205553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409444.434259854] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409444.434315530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.434148025] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409445.434202869] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.783414939] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.334387877] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.434180812] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.434229184] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.892924859] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.892967801] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.099077143] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.099173547] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.434130389] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409447.434225630] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.434186935] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409448.434235297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.595842484] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.614335620] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.165297978] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.433943105] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.433998400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.721228677] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.721281176] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.433999883] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409450.434038716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409451.434230884] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409451.434307500] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409452.434199919] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409452.434253361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409453.433989726] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409453.434051784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409454.434154454] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409454.434230248] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409455.434100464] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409455.434166469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409456.434081412] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409456.434132860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409457.434134292] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409457.434197072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409458.433948272] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409458.434003066] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [INFO] [1778409459.434215665] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [ros2_control_node-1] [WARN] [1778409459.434271941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [1778409432.949648868] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] [INFO] [1778409460.036698451] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:37:40 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:18 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] * Running on http://127.0.0.1:46513 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] * Running on http://172.30.43.138:46513 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:18] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.396550516] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.396607525] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.396623695] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.400874802] [moveit_1090737569.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.403194570] [moveit_1090737569.moveit.ros.rdf_loader]: Loaded robot model in 0.00216529 seconds (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.403227893] [moveit_1090737569.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.403236991] [moveit_1090737569.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [WARN] [1778409442.415048715] [moveit_1090737569.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.417972099] [moveit_1090737569.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.479782616] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.480010600] [moveit_1090737569.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481120994] [moveit_1090737569.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481598101] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481612919] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.481748312] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482148212] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482233524] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482733814] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.482749013] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.483157780] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.483591203] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [WARN] [1778409442.483949273] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409442.483970414] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.973274624] [moveit_1090737569.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.974414961] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.975805263] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.975824369] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976189643] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976205373] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976239728] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976250439] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976261259] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.976991126] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.979382871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.979395244] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.981620287] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.981636437] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409442.982358480] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.018995259] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.022934028] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.023103079] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.023132004] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409443.023812477] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:24] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  Generated 1 grasp options for object Mesh1. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:25 INFO  IK accepted: 1 grasp options. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016513833] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016631015] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016644711] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.016663096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [WARN] [1778409447.016731406] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.017212781] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.017309254] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.034331148] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.034818875] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.035063309] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Executing plan (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096413186] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096470325] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096503067] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096511844] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.096544025] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098332793] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098379933] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098406062] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.098538494] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.099385574] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409447.099409519] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409448.600248882] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409448.606065885] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:29 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.757943972] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.757994779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758002173] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758011360] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [WARN] [1778409449.758072987] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758319315] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409449.758379840] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.762259084] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.762469064] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.763025205] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.763156835] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [WARN] [1778409459.763303860] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004847 seconds (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.767335099] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [ERROR] [1778409459.767395965] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:39 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 127.0.0.1 - - [10/May/2026 12:37:39] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409460.166867386] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:55 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:55 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] 2026-05-10 12:37:55 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:37:57 [ ERROR] [INFO] [1778409475.300799510] [moveit_1090737569.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________ test_pick_and_place_mesh_by_id[ur5e] _____________________ - -start_processes = Urls(ros_domain_id='48', robot_url='http://0.0.0.0:46513', storage_url='http://127.0.0.1:51823') - - @pytest.mark.timeout(321) - def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None: - assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" -  - storage_client.URL = start_processes.storage_url -  - storage_client.create_asset( - MESH_ASSET_ID, - MESH_PATH.read_bytes(), - description="Test mesh for UR5e collision avoidance.", - ) -  - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Mesh("Mesh1", MESH_ASSET_ID) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:45: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Mesh1. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-09-975076-DESKTOP-HG3Q5KV-9294 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [9301] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [9302] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [9303] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [9304] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [9305] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409430.283072484] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.290127024] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.293110835] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.295385617] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.295408190] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.295417127] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.295502138] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.460250108] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.463333817] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.463758784] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.463848415] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.473434458] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475031773] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475068273] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475098510] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475105183] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475187319] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.475240019] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.575491178] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.827882954] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.827937067] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.830258218] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.842762256] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.843890946] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.844004481] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.851256036] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.852075537] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.854937024] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.855728088] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.855761962] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.860790948] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.875133631] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.875395533] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.876392513] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.888327004] [controller_manager]: Overrun might occur, Total time : 10500.567 us (Expected < 2000.000 us) --> Read time : 12.654 us, Update time : 10485.718 us (Switch time : 10431.786 us (Switch chained mode time : 0.421 us, perform mode change time : 16.111 us, Activation time : 10400.527 us, Deactivation time : 0.320 us)), Write time : 2.195 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.888397017] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 10.681132 ms (missed cycles : 6). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.888336377] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.890746205] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.891579003] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.891632404] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.892101676] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.902523645] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.904737415] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.905006041] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.905051728] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.906578349] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.907950646] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.910702891] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.911613246] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.911647551] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.912672869] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.926847733] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.927110793] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.929276157] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.930672471] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.931920958] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.934792514] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.935645219] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.935680917] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.935906376] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.948847494] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.949093797] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.952600080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.954013912] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.956894364] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.958003287] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.958038324] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.958328104] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.969527479] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.981286372] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981577581] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981811977] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981868745] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.981971560] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.984779390] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.992642003] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.994081759] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409430.997018094] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.083562408] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 9304] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.336052076] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.336110427] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.336471422] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.361236664] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362605320] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362759884] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362797024] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.362810289] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.364030217] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.371276932] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.371317229] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.372861593] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.389025791] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.389365848] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.390468733] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.393397488] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.393434449] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.394269941] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.411003271] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.411368926] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.412190507] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.415473695] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.415513420] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.416306347] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.433069809] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.433410166] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.434035424] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.437238225] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.437270206] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.437555177] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.452842175] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.453169652] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.463384456] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.463447686] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.463758938] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.478987382] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.479295043] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.485501135] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.485546211] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.485877835] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.500733118] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.501059497] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.505489947] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.505525755] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.505784477] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_6vcof4q5 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409431.520771271] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.521083360] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 9305] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409432.763700913] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9302] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.073391159] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409433.073445442] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.992241141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.526579 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.161177547] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 5.462914 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.246199523] [controller_manager]: Overrun might occur, Total time : 2426.716 us (Expected < 2000.000 us) --> Read time : 12.864 us, Update time : 2412.239 us, Write time : 1.613 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.006216951] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.502268 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409439.434062068] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.434115389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409440.434129650] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409440.434239659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409441.434104073] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409441.434166212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409442.434163204] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409442.434230552] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409443.434155498] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409443.434205553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409444.434259854] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409444.434315530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.434148025] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409445.434202869] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.783414939] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.334387877] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.434180812] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.434229184] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.892924859] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.892967801] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.099077143] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.099173547] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.434130389] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409447.434225630] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.434186935] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409448.434235297] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.595842484] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.614335620] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.165297978] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.433943105] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.433998400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.721228677] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.721281176] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.433999883] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409450.434038716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409451.434230884] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409451.434307500] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409452.434199919] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409452.434253361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409453.433989726] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409453.434051784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409454.434154454] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409454.434230248] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409455.434100464] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409455.434166469] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409456.434081412] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409456.434132860] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409457.434134292] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409457.434197072] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409458.433948272] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409458.434003066] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409459.434215665] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409459.434271941] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409432.949648868] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409460.036698451] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:46513 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:46513 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:18] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.396550516] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.396607525] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.396623695] [moveit_1090737569.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.400874802] [moveit_1090737569.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.403194570] [moveit_1090737569.moveit.ros.rdf_loader]: Loaded robot model in 0.00216529 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.403227893] [moveit_1090737569.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.403236991] [moveit_1090737569.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409442.415048715] [moveit_1090737569.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.417972099] [moveit_1090737569.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.479782616] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.480010600] [moveit_1090737569.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481120994] [moveit_1090737569.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481598101] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481612919] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.481748312] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482148212] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482233524] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482733814] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.482749013] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.483157780] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.483591203] [moveit_1090737569.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409442.483949273] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409442.483970414] [moveit_1090737569.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.973274624] [moveit_1090737569.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.974414961] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.975805263] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.975824369] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976189643] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976205373] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976239728] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976250439] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976261259] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.976991126] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.979382871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.979395244] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.981620287] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.981636437] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409442.982358480] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.018995259] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.022934028] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.023103079] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.023132004] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409443.023812477] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:24] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:24] "PUT /collisions/mesh?meshId=Mesh1&meshFileId=triangle_block.stl HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Generated 1 grasp options for object Mesh1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  IK accepted: 1 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016513833] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016631015] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016644711] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.016663096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409447.016731406] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.017212781] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.017309254] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.034331148] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.034818875] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.035063309] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096413186] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096470325] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096503067] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096511844] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.096544025] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098332793] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098379933] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098406062] [moveit_1090737569.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.098538494] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.099385574] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409447.099409519] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409448.600248882] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409448.606065885] [moveit_1090737569.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.757943972] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.757994779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758002173] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758011360] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409449.758072987] [moveit_1090737569.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758319315] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409449.758379840] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.762259084] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.762469064] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.763025205] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.763156835] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409459.763303860] [moveit_1090737569.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004847 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.767335099] [moveit_1090737569.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409459.767395965] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Mesh1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:37:39] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409460.166867386] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409475.300799510] [moveit_1090737569.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:18 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:18: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_mesh_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py::test_pick_and_place_mesh_by_id[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... -======================== 1 failed, 1 warning in 48.46s ========================= - - -12:38:05.70 [INFO] Long running tasks: - 114.20s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - 161.18s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 327.50s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:38:34.30 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests -12:38:34.30 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-fzT5zi -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py::test_pick_and_place_box_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:38:15 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-12-775176-DESKTOP-HG3Q5KV-8237 (testutils.py:33) - -2026-05-10 12:38:15 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [8241] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [8242] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [8243] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-4]: process started with pid [8244] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-5]: process started with pid [8245] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [robot_state_publisher-2] [INFO] [1778409373.068470101] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.076757668] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.078945967] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.081048862] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.081078107] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.081083438] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.081172412] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.249956242] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.252875759] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.253289074] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.253363005] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.261915600] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263190413] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263223836] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263247421] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263253162] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263309950] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.263346038] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.331357815] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.583676046] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.583735639] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.586388345] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.598535029] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.599607823] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.599735295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.606987490] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.607730313] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.610729356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.611519948] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.611574221] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.616268330] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.628869685] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.629221808] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.630365596] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.640653570] [controller_manager]: Overrun might occur, Total time : 9127.132 us (Expected < 2000.000 us) --> Read time : 11.482 us, Update time : 9113.376 us (Switch time : 9069.974 us (Switch chained mode time : 0.691 us, perform mode change time : 10.730 us, Activation time : 9042.732 us, Deactivation time : 0.241 us)), Write time : 2.274 us (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.640717141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.267481 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.640688923] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.642593517] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.643445300] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.643514762] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.643853345] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.654957715] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.656440718] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.656805591] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.656868110] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.658458893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.659681947] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.662675284] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.663656148] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.663699851] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.664813106] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.678640194] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.678907211] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.681066889] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.682544553] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.683650093] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.686513760] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.687350324] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.687393847] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.687702233] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.700494575] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.700766100] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.704371837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.705663852] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.708642497] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.709601464] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.709636882] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.710089627] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.719856270] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.730778994] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731025467] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731133803] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731147779] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.731165744] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.733165730] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.740540753] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.741810335] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-4] [INFO] [1778409373.744662840] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409373.840375727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 8244] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.092950336] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.093002665] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.093401453] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.116561043] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117651330] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117857582] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117905323] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.117925742] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.119249121] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.126820173] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.126865879] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.128118965] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.142724541] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.143058941] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.143915279] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.147026466] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.147087622] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.147818150] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.162508088] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.162920862] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.163553364] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.167047913] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.167068923] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.167692318] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.182669251] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.183004232] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.183532987] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.187261066] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.187292927] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.187606342] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.202561279] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.202916749] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.213354532] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.213422621] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.213996032] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.230750356] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.231101208] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.237313998] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.237349826] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.237678861] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.255124435] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.255464111] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.260298752] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.260333508] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.260973094] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [spawner-5] [INFO] [1778409374.277101960] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.277469503] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 8245] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [robot_state_publisher-2] [INFO] [1778409375.566933184] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8242] (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409375.904389573] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409375.904439778] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.798204480] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.755280 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.912496947] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.047718 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.758215760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.766491 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.942173203] [controller_manager]: Overrun might occur, Total time : 2663.261 us (Expected < 2000.000 us) --> Read time : 14.187 us, Update time : 2647.420 us, Write time : 1.654 us (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409381.207973435] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409381.208037948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409382.208230550] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409382.208400292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409383.208152500] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409383.208196203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409384.208236134] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409384.208277423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409385.208235344] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409385.208283926] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409386.208114213] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409386.208160211] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.208203329] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.208251460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409388.208092156] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409388.208135619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409389.208138014] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409389.208184392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409390.208144281] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409390.208194577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409391.208143880] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409391.208195268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409392.208231832] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409392.208282568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409393.208393525] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409393.208446215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409394.208162331] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409394.208211264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409395.208510539] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.208564942] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.910220010] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.770891 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409396.208224124] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409396.208273097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.208321200] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.208392245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.208189670] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409398.208238914] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409399.208266438] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409399.208315260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409400.208408999] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409400.208480936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409401.208239177] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.208283662] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.864181712] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.732663 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409402.208164936] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409402.208221203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.049880968] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431969 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409403.208251296] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.208323042] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409404.208159046] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409404.208205745] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409405.208386440] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.208441595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409405.833708418] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.866272614] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.823535 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.904120714] [controller_manager]: Overrun might occur, Total time : 2612.533 us (Expected < 2000.000 us) --> Read time : 15.289 us, Update time : 2595.401 us, Write time : 1.843 us (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409406.208160095] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.208213286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.384648681] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.945869051] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409406.945927943] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.072700416] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.072771231] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.208100689] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.208166464] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409408.208430366] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409408.208520117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409409.208315094] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409409.208356342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.035571210] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.208238760] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409410.208276993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409411.208347904] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409411.208403339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409412.207998256] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409412.208044494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409413.208235624] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409413.208288886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409414.208186725] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409414.208241699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409415.208100284] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409415.208161811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409416.208188421] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409416.208238366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409417.208108093] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409417.208153960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409418.208174207] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409418.208224833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409419.208063056] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.208115084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.208175989] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409420.208220934] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409421.208082611] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409421.208130352] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409422.208059911] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409422.208112380] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409423.208094003] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409423.208140001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409424.208077510] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409424.208129348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409425.159857961] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409425.208084436] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409425.208116898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409425.710863333] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.208332450] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.208382816] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.283054049] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.283105497] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.357607274] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.908604516] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.208183836] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409427.208232157] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409427.485536232] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.485586036] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.572615446] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.572686180] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409428.208101992] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409428.208170673] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.070218200] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.768971 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.163844887] [controller_manager]: Overrun might occur, Total time : 2313.205 us (Expected < 2000.000 us) --> Read time : 14.788 us, Update time : 2296.493 us, Write time : 1.924 us (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409429.208279066] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.208327208] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.208135602] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.208179576] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.208169584] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409431.208222936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409432.208307680] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409432.208365340] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.179578786] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.208124280] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409433.208175217] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409434.208082455] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.208151797] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.992153414] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.704075 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409435.208102301] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409435.208153508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.160550956] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.101477 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409436.209810664] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.210233127] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.243112903] [controller_manager]: Overrun might occur, Total time : 3603.567 us (Expected < 2000.000 us) --> Read time : 16.331 us, Update time : 3585.573 us, Write time : 1.663 us (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409437.208327254] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409437.208384873] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409438.208194875] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409438.208243878] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.006218013] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.768644 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409439.208327329] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.208383777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409440.208181276] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409440.208244716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409441.208260799] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409441.208292339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409442.208153169] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409442.208199136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409443.208129406] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409443.208177828] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409444.208187906] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409444.208239504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.208072982] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409445.208124479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.208028274] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.208071116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.208307154] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409447.208366907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.208263683] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409448.208324499] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.208083957] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.208134082] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.208159408] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409450.208210505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409451.208045591] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409451.208098170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409452.208287028] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409452.208339187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409453.208046984] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409453.208115754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409454.208147755] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409454.208215954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409455.208113984] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409455.208164811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409456.208219539] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409456.208272219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409457.208183345] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409457.208232207] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409458.208065719] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409458.208136334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409459.208151737] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409459.208197524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409460.208171064] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409460.208214726] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409461.208113353] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409461.208161234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409462.208316210] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409462.208371424] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409463.208187786] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409463.208234154] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409463.381260925] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [WARN] [1778409463.932328201] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:15 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.208038700] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409464.208089156] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409464.573984284] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.574040200] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.616489132] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409464.616551380] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409465.063583848] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409465.207856519] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409465.207902386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409466.208269664] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409466.208318647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409467.208149742] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409467.208201169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409468.208095074] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409468.208144238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409469.208041162] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409469.208089283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409470.208109626] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409470.208159300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409471.208109239] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409471.208157300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409472.208077962] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409472.208127757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409473.208094167] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409473.208145274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409474.208088029] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409474.208142743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409475.208274322] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409475.208326692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409476.208252324] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409476.208303751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409477.208258901] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409477.208313976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409478.208233463] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409478.208286785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409478.452754276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.304856 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409478.466182907] [controller_manager]: Overrun might occur, Total time : 4667.613 us (Expected < 2000.000 us) --> Read time : 17.142 us, Update time : 4648.698 us, Write time : 1.773 us (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.208282072] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409479.208334572] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.208296085] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409480.208337614] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409481.208100506] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409481.208157784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409482.208324476] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409482.208375784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409483.208055281] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409483.208105046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409484.208597809] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409484.208637515] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409484.330204175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.754975 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409485.208322832] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.208379920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.470149161] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.699511 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.522270903] [controller_manager]: Overrun might occur, Total time : 6743.634 us (Expected < 2000.000 us) --> Read time : 44.725 us, Update time : 6697.156 us, Write time : 1.753 us (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409486.208255164] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409486.208306472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409487.208007464] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409487.208052730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409488.208230615] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.208285830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.298204230] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.755011 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409489.208210063] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409489.208262883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409490.208179532] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409490.208231560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409491.208174759] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409491.208213031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409492.208135455] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409492.208176182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409493.208062761] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409493.208102867] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409494.208277176] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409494.208331540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [INFO] [1778409495.208033988] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [ros2_control_node-1] [WARN] [1778409495.208078382] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [INFO] [1778409375.780969008] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] [INFO] [1778409399.647001065] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:38:16 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:20 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] * Running on http://127.0.0.1:36055 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] * Running on http://172.30.43.138:36055 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:20] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.405448672] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.405503687] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.405521631] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.412114729] [moveit_2647972108.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.413770896] [moveit_2647972108.moveit.ros.rdf_loader]: Loaded robot model in 0.00151281 seconds (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.413808518] [moveit_2647972108.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.413828556] [moveit_2647972108.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [WARN] [1778409384.424866358] [moveit_2647972108.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.427456970] [moveit_2647972108.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.478803576] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.478919366] [moveit_2647972108.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.479729225] [moveit_2647972108.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480292780] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480309913] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480465543] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480880983] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.480971996] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.481670888] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.481696908] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.482234088] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.482723729] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [WARN] [1778409384.482956271] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409384.482968073] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.527493633] [moveit_2647972108.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.528237797] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529510455] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529526215] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529821711] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529837721] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529864743] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529870443] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.529888428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.531002975] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.533789761] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.533802846] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.536449044] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.536465936] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.537114729] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.568789650] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.573106892] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.573266205] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.573291824] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409384.574089895] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:26] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:27 INFO  Generated 12 grasp options for object Box1. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.677263032] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.677294392] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.679067761] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_cup_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.679088891] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.680610343] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.680629269] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.682502829] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'upper_arm_link' (type 'Robot link') and 'base_link_inertia' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409387.682546963] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409405.828984722] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409405.829038624] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:45 INFO  IK accepted: 4 grasp options. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:46 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952698584] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952818542] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952832118] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.952850793] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [WARN] [1778409406.952940634] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.953372465] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409406.953467114] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.045556509] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.047100473] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.047485034] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:47 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071423802] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071452727] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071480068] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071490248] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071513001] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071926846] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.071998503] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.072029251] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.072168035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.073002295] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409407.073021271] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409410.073882431] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409410.079847045] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:36:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:06 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298506491] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298583136] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298598606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.298616901] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [WARN] [1778409426.298701461] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.299020367] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.299086382] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.351725268] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.354094790] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409426.355060265] [moveit_2647972108.moveit.ros.validate_solution]: Computed path is not valid. Invalid states at index locations: [ 18 ] out of 36. Explanations follow in command line. Contacts are published on /display_contacts (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.355139636] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409426.355148633] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409426.355214007] [moveit_2647972108.moveit.ros.validate_solution]: Completed listing of explanations for invalid states. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [ERROR] [1778409426.355324908] [moveit_py]: PlanningResponseAdapter 'ValidateSolution' failed with error code INVALID_MOTION_PLAN (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502115930] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502172828] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502181494] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502191162] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [WARN] [1778409427.502255665] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502472667] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.502526279] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.563656258] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.568405908] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.570404916] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:07 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571485134] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571507376] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571532554] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571540288] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571561198] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571900914] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571949136] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.571975305] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.572147222] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.572991416] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409427.573003619] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409433.223285935] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409433.227866574] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:28 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591183202] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591257764] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591270167] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591283633] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [WARN] [1778409464.591371059] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591653606] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.591703391] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.612487102] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.612603664] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.612775600] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:44 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613587122] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613612140] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613631426] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613639822] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.613659800] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.615902728] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.615938987] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.615959616] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.616078472] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.616813157] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409464.616825080] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409465.067224286] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409465.071801473] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:37:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:00 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:15 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:15] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409496.020399484] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:31 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:31 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] 2026-05-10 12:38:31 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:38:33 [ ERROR] [INFO] [1778409511.164291488] [moveit_2647972108.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_____________________ test_pick_and_place_box_by_id[ur5e] ______________________ - -start_processes = Urls(ros_domain_id='19', robot_url='http://0.0.0.0:36055', storage_url='http://127.0.0.1:52557') - - @pytest.mark.timeout(400) - def test_pick_and_place_box_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Box("Box1", 0.2, 0.2, 0.2) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0.5, 0.5, 0.5, 0)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id - rest.call( -src/python/arcor2_web/rest.py:273: in call - _handle_response(resp) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -resp = - - def _handle_response(resp: requests.Response) -> None: -  """Raises exception if there is something wrong with the response. -  -  :param resp: -  :return: -  """ -  - if resp.status_code < 400: - return -  - decoded_content = resp.content.decode() -  - # here we try to handle different cases - try: - cont = json.loads(decoded_content) - except json.JsonException: - # response contains invalid JSON - raise RestException(decoded_content) -  - if isinstance(cont, str): # just plain text - raise RestException(cont) - elif isinstance(cont, dict): # this could be WebApiError - try: -> raise WebApiError.from_dict(cont) -E arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach object Box1. - -src/python/arcor2_web/rest.py:340: WebApiError ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-36-12-775176-DESKTOP-HG3Q5KV-8237 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [8241] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [8242] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [8243] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [8244] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [8245] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409373.068470101] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.076757668] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.078945967] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.081048862] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.081078107] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.081083438] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.081172412] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.249956242] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.252875759] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.253289074] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.253363005] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.261915600] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263190413] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263223836] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263247421] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263253162] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263309950] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.263346038] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.331357815] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.583676046] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.583735639] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.586388345] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.598535029] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.599607823] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.599735295] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.606987490] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.607730313] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.610729356] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.611519948] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.611574221] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.616268330] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.628869685] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.629221808] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.630365596] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.640653570] [controller_manager]: Overrun might occur, Total time : 9127.132 us (Expected < 2000.000 us) --> Read time : 11.482 us, Update time : 9113.376 us (Switch time : 9069.974 us (Switch chained mode time : 0.691 us, perform mode change time : 10.730 us, Activation time : 9042.732 us, Deactivation time : 0.241 us)), Write time : 2.274 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.640717141] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.267481 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.640688923] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.642593517] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.643445300] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.643514762] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.643853345] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.654957715] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.656440718] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.656805591] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.656868110] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.658458893] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.659681947] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.662675284] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.663656148] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.663699851] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.664813106] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.678640194] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.678907211] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.681066889] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.682544553] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.683650093] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.686513760] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.687350324] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.687393847] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.687702233] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.700494575] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.700766100] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.704371837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.705663852] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.708642497] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.709601464] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.709636882] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.710089627] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.719856270] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.730778994] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731025467] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731133803] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731147779] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.731165744] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.733165730] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.740540753] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.741810335] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409373.744662840] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409373.840375727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 8244] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.092950336] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.093002665] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.093401453] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.116561043] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117651330] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117857582] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117905323] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.117925742] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.119249121] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.126820173] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.126865879] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.128118965] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.142724541] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.143058941] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.143915279] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.147026466] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.147087622] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.147818150] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.162508088] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.162920862] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.163553364] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.167047913] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.167068923] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.167692318] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.182669251] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.183004232] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.183532987] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.187261066] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.187292927] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.187606342] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.202561279] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.202916749] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.213354532] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.213422621] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.213996032] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.230750356] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.231101208] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.237313998] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.237349826] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.237678861] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.255124435] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.255464111] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.260298752] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.260333508] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.260973094] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_3svrvxyc -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409374.277101960] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.277469503] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 8245] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409375.566933184] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 8242] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409375.904389573] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409375.904439778] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.798204480] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.755280 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.912496947] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.047718 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.758215760] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.766491 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.942173203] [controller_manager]: Overrun might occur, Total time : 2663.261 us (Expected < 2000.000 us) --> Read time : 14.187 us, Update time : 2647.420 us, Write time : 1.654 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409381.207973435] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409381.208037948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409382.208230550] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409382.208400292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409383.208152500] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409383.208196203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409384.208236134] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409384.208277423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409385.208235344] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409385.208283926] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409386.208114213] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409386.208160211] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.208203329] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.208251460] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409388.208092156] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409388.208135619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409389.208138014] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409389.208184392] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409390.208144281] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409390.208194577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409391.208143880] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409391.208195268] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409392.208231832] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409392.208282568] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409393.208393525] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409393.208446215] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409394.208162331] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409394.208211264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409395.208510539] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.208564942] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.910220010] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.770891 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409396.208224124] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409396.208273097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.208321200] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.208392245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.208189670] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409398.208238914] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409399.208266438] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409399.208315260] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409400.208408999] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409400.208480936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409401.208239177] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.208283662] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.864181712] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.732663 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409402.208164936] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409402.208221203] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.049880968] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.431969 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409403.208251296] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.208323042] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409404.208159046] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409404.208205745] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409405.208386440] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.208441595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409405.833708418] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.866272614] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.823535 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.904120714] [controller_manager]: Overrun might occur, Total time : 2612.533 us (Expected < 2000.000 us) --> Read time : 15.289 us, Update time : 2595.401 us, Write time : 1.843 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409406.208160095] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.208213286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.384648681] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.945869051] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409406.945927943] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.072700416] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.072771231] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.208100689] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.208166464] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409408.208430366] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409408.208520117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409409.208315094] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409409.208356342] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.035571210] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.208238760] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409410.208276993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409411.208347904] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409411.208403339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409412.207998256] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409412.208044494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409413.208235624] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409413.208288886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409414.208186725] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409414.208241699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409415.208100284] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409415.208161811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409416.208188421] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409416.208238366] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409417.208108093] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409417.208153960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409418.208174207] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409418.208224833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409419.208063056] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.208115084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.208175989] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409420.208220934] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409421.208082611] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409421.208130352] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409422.208059911] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409422.208112380] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409423.208094003] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409423.208140001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409424.208077510] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409424.208129348] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409425.159857961] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409425.208084436] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409425.208116898] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409425.710863333] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.208332450] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.208382816] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.283054049] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.283105497] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.357607274] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.908604516] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.208183836] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409427.208232157] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409427.485536232] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.485586036] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.572615446] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.572686180] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409428.208101992] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409428.208170673] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.070218200] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.768971 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.163844887] [controller_manager]: Overrun might occur, Total time : 2313.205 us (Expected < 2000.000 us) --> Read time : 14.788 us, Update time : 2296.493 us, Write time : 1.924 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409429.208279066] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.208327208] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.208135602] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.208179576] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.208169584] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409431.208222936] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409432.208307680] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409432.208365340] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.179578786] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.208124280] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409433.208175217] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409434.208082455] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.208151797] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.992153414] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.704075 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409435.208102301] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409435.208153508] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.160550956] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.101477 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409436.209810664] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.210233127] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.243112903] [controller_manager]: Overrun might occur, Total time : 3603.567 us (Expected < 2000.000 us) --> Read time : 16.331 us, Update time : 3585.573 us, Write time : 1.663 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409437.208327254] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409437.208384873] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409438.208194875] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409438.208243878] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.006218013] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.768644 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409439.208327329] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.208383777] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409440.208181276] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409440.208244716] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409441.208260799] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409441.208292339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409442.208153169] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409442.208199136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409443.208129406] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409443.208177828] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409444.208187906] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409444.208239504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.208072982] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409445.208124479] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.208028274] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.208071116] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.208307154] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409447.208366907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.208263683] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409448.208324499] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.208083957] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.208134082] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.208159408] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409450.208210505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409451.208045591] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409451.208098170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409452.208287028] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409452.208339187] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409453.208046984] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409453.208115754] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409454.208147755] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409454.208215954] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409455.208113984] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409455.208164811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409456.208219539] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409456.208272219] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409457.208183345] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409457.208232207] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409458.208065719] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409458.208136334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409459.208151737] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409459.208197524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409460.208171064] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409460.208214726] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409461.208113353] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409461.208161234] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409462.208316210] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409462.208371424] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409463.208187786] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409463.208234154] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409463.381260925] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409463.932328201] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.208038700] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409464.208089156] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409464.573984284] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.574040200] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.616489132] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409464.616551380] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409465.063583848] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409465.207856519] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409465.207902386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409466.208269664] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409466.208318647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409467.208149742] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409467.208201169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409468.208095074] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409468.208144238] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409469.208041162] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409469.208089283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409470.208109626] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409470.208159300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409471.208109239] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409471.208157300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409472.208077962] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409472.208127757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409473.208094167] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409473.208145274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409474.208088029] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409474.208142743] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409475.208274322] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409475.208326692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409476.208252324] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409476.208303751] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409477.208258901] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409477.208313976] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409478.208233463] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409478.208286785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409478.452754276] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.304856 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409478.466182907] [controller_manager]: Overrun might occur, Total time : 4667.613 us (Expected < 2000.000 us) --> Read time : 17.142 us, Update time : 4648.698 us, Write time : 1.773 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.208282072] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409479.208334572] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.208296085] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409480.208337614] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409481.208100506] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409481.208157784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409482.208324476] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409482.208375784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409483.208055281] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409483.208105046] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409484.208597809] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409484.208637515] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409484.330204175] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.754975 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409485.208322832] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.208379920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.470149161] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.699511 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.522270903] [controller_manager]: Overrun might occur, Total time : 6743.634 us (Expected < 2000.000 us) --> Read time : 44.725 us, Update time : 6697.156 us, Write time : 1.753 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409486.208255164] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409486.208306472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409487.208007464] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409487.208052730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409488.208230615] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.208285830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.298204230] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.755011 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409489.208210063] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409489.208262883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409490.208179532] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409490.208231560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409491.208174759] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409491.208213031] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409492.208135455] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409492.208176182] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409493.208062761] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409493.208102867] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409494.208277176] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409494.208331540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409495.208033988] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409495.208078382] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409375.780969008] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409399.647001065] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:20 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:36055 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:36055 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:20] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.405448672] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.405503687] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.405521631] [moveit_2647972108.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.412114729] [moveit_2647972108.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.413770896] [moveit_2647972108.moveit.ros.rdf_loader]: Loaded robot model in 0.00151281 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.413808518] [moveit_2647972108.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.413828556] [moveit_2647972108.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409384.424866358] [moveit_2647972108.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.427456970] [moveit_2647972108.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.478803576] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.478919366] [moveit_2647972108.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.479729225] [moveit_2647972108.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480292780] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480309913] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480465543] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480880983] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.480971996] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.481670888] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.481696908] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.482234088] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.482723729] [moveit_2647972108.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409384.482956271] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409384.482968073] [moveit_2647972108.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.527493633] [moveit_2647972108.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.528237797] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529510455] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529526215] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529821711] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529837721] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529864743] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529870443] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.529888428] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.531002975] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.533789761] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.533802846] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.536449044] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.536465936] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.537114729] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.568789650] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.573106892] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.573266205] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.573291824] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409384.574089895] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:26] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:36:26] "PUT /collisions/box?sizeX=0.2&sizeY=0.2&sizeZ=0.2&boxId=Box1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Generated 12 grasp options for object Box1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.677263032] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.677294392] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.679067761] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_cup_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.679088891] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.680610343] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.680629269] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.682502829] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'upper_arm_link' (type 'Robot link') and 'base_link_inertia' (type 'Robot link'), which constitutes a collision. Contact information is not stored. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.682546963] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409405.828984722] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'suction_cup_link' (type 'Robot link') and 'forearm_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409405.829038624] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  IK accepted: 4 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952698584] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952818542] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952832118] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.952850793] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409406.952940634] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.953372465] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409406.953467114] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.045556509] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.047100473] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.047485034] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071423802] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071452727] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071480068] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071490248] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071513001] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071926846] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.071998503] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.072029251] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.072168035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.073002295] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409407.073021271] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.073882431] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409410.079847045] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298506491] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298583136] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298598606] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.298616901] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409426.298701461] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.299020367] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.299086382] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.351725268] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.354094790] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409426.355060265] [moveit_2647972108.moveit.ros.validate_solution]: Computed path is not valid. Invalid states at index locations: [ 18 ] out of 36. Explanations follow in command line. Contacts are published on /display_contacts -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.355139636] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Found a contact between 'forearm_link' (type 'Robot link') and 'suction_base_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409426.355148633] [moveit_2647972108.moveit.core.moveit_collision_detection_fcl]: Collision checking is considered complete (collision was found and 0 contacts are stored) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409426.355214007] [moveit_2647972108.moveit.ros.validate_solution]: Completed listing of explanations for invalid states. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409426.355324908] [moveit_py]: PlanningResponseAdapter 'ValidateSolution' failed with error code INVALID_MOTION_PLAN -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502115930] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502172828] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502181494] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502191162] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409427.502255665] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502472667] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.502526279] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.563656258] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.568405908] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.570404916] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571485134] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571507376] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571532554] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571540288] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571561198] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571900914] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571949136] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.571975305] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.572147222] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.572991416] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409427.573003619] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409433.223285935] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409433.227866574] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591183202] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591257764] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591270167] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591283633] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409464.591371059] [moveit_2647972108.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591653606] [moveit_2647972108.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.591703391] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.612487102] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.612603664] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.612775600] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:44 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613587122] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613612140] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613631426] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613639822] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.613659800] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.615902728] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.615938987] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.615959616] [moveit_2647972108.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.616078472] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.616813157] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409464.616825080] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409465.067224286] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409465.071801473] [moveit_2647972108.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:00 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Box1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:15] "PUT /graspable/pick_up_object_by_id HTTP/1.1" 500 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409496.020399484] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:31 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:31 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:31 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409511.164291488] [moveit_2647972108.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(400) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_rotated_box.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py::test_pick_and_place_box_by_id[ur5e] - arcor2.data.common.WebApiError: arcor2_ur (UrGeneral): Unable to attach obj... -=================== 1 failed, 1 warning in 141.78s (0:02:21) =================== - - -12:38:35.74 [INFO] Long running tasks: - 191.22s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - 357.54s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:38:48.71 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests -12:38:48.71 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-EXxbd3 -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py::test_pick_and_place_sphere_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:37:42 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-35-25-644363-DESKTOP-HG3Q5KV-7301 (testutils.py:33) - -2026-05-10 12:37:42 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [7305] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [7306] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [7307] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-4]: process started with pid [7308] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-5]: process started with pid [7309] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [robot_state_publisher-2] [INFO] [1778409325.942210256] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.947799250] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.950736577] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.953197293] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.953231608] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409325.953242869] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409325.953350278] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.120336732] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.123018868] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.123324519] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.123385605] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.130833908] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132086789] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132119311] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132143697] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132148466] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132218880] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.132264066] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.219184727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.471755962] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.471859349] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.475205731] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.501292831] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.502905389] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.503185982] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.503235807] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.503268659] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.509578615] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.516867314] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.516922709] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.518092171] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.531235480] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.531527279] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.532303705] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.534892644] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.534953159] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.535888250] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.549227422] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.549550246] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.550219006] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.553425880] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.553492536] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.554308937] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.568900458] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.569222675] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.569770275] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.572971619] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.573014150] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.577406760] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.590761463] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.591060816] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.601405402] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.601457040] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.601886996] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.615404792] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.615737239] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.621544348] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.621584614] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.622001422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.636973710] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.637285186] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.641542316] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.641599645] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.642291695] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-5] [INFO] [1778409326.659198425] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409326.659560593] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409326.781022611] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 7309] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.033381443] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.033434163] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.034814501] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.049050331] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.050262339] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.050331069] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.055115496] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.055939295] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.058943580] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.059809093] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.059851494] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.060220620] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.074733632] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.075025111] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.076317170] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.086903782] [controller_manager]: Overrun might occur, Total time : 9174.573 us (Expected < 2000.000 us) --> Read time : 13.265 us, Update time : 9159.004 us (Switch time : 9112.685 us (Switch chained mode time : 0.381 us, perform mode change time : 10.590 us, Activation time : 9086.385 us, Deactivation time : 0.140 us)), Write time : 2.304 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409327.086970298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.369513 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.086920683] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.088776741] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.089966326] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.089995522] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.090479456] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.103155146] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.104523251] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.104833771] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.104891340] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.106364179] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.107781928] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.110607070] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.111831933] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.111868602] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.112987494] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.130674112] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.130986585] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.133232552] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.134426440] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.135838925] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.138782207] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.140021839] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.140057276] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.140286040] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.154638114] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.154913282] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.158551080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.159850925] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.163190305] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.164577321] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.164606987] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.164915623] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.178563660] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.195106183] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195644772] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195819314] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195839432] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.195850523] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.197441006] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.204700484] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409327.205932341] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [spawner-4] [INFO] [1778409327.209285311] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 7308] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [robot_state_publisher-2] [INFO] [1778409328.446955673] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7306] (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409328.739644512] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409328.739702071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409330.678208112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.607607 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409331.853933576] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.333152 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409334.644270826] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.670582 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409335.168492845] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409335.168542008] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409336.168772539] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409336.168826491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409337.168743000] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409337.168798535] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409338.168720250] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409338.168795212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.109685823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.085299 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409339.168899071] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409339.168946251] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409340.168841029] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409340.168888549] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409341.168868701] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409341.168944595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.125686004] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409342.168751679] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.168832081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409342.676798513] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.168854544] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409343.168909579] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409343.259686463] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.259741808] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.654803816] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409343.654870663] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409344.168600452] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409344.168650137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.125736596] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.168692507] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409345.168736170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409345.331307588] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409345.882331048] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409346.168565487] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409346.168623286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409346.461173069] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409346.461215430] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409347.168623127] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409347.168675997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409348.168598029] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409348.168652983] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409349.168618695] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409349.168677768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409350.168572337] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409350.168640396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409351.168651363] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409351.168701368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409352.168680082] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409352.168736830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409353.168612348] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409353.168682622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409354.168627087] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409354.168701629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409355.168722153] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409355.168785623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409356.168614422] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409356.168682111] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409356.589691529] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409357.140678175] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409357.168566571] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409357.168615704] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409357.837277912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409357.837336223] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.168591348] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409358.168642565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.528516719] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.528591521] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.731702722] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409358.788355279] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409359.168687876] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409359.168743141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409359.339379391] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409359.959512833] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409359.959563079] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409360.168877187] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409360.168927893] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409361.168837946] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409361.168890947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409361.618218898] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.618494 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409361.714251653] [controller_manager]: Overrun might occur, Total time : 2590.437 us (Expected < 2000.000 us) --> Read time : 13.196 us, Update time : 2575.488 us, Write time : 1.753 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409362.168603671] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409362.168652564] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409363.168658992] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409363.168731670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409364.168563690] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409364.168614857] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409365.168670830] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409365.168725093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409366.168697603] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409366.168775061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409367.168768120] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.168819247] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.478199889] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.599615 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409367.504896572] [controller_manager]: Overrun might occur, Total time : 3234.988 us (Expected < 2000.000 us) --> Read time : 13.957 us, Update time : 3219.458 us, Write time : 1.573 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409368.168910775] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.168961120] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.726165594] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.564969 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409368.768172530] [controller_manager]: Overrun might occur, Total time : 2510.060 us (Expected < 2000.000 us) --> Read time : 13.867 us, Update time : 2494.610 us, Write time : 1.583 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409369.169061715] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409369.169118292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409370.018611113] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409370.168804323] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409370.168855200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409370.569586880] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.126530371] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.126580216] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.150843629] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.150901199] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.168786939] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.168861501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.520114881] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.514126 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409371.551684010] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409371.910258652] [controller_manager]: Overrun might occur, Total time : 2469.109 us (Expected < 2000.000 us) --> Read time : 666.808 us, Update time : 1800.157 us, Write time : 2.144 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409372.168876708] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409372.168932374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409373.168777523] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409373.168829060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409374.168775783] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409374.168819696] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409375.168705212] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409375.168763553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409376.168666972] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409376.168718791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409377.168645199] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.168695625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409377.798227590] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.627356 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409378.168606515] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.168658534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.912603861] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.003546 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409378.932849665] [controller_manager]: Overrun might occur, Total time : 3190.701 us (Expected < 2000.000 us) --> Read time : 13.475 us, Update time : 3175.673 us, Write time : 1.553 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409379.168561076] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409379.168616782] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409380.168611025] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.168664727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.758219147] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.618923 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409380.906390507] [controller_manager]: Overrun might occur, Total time : 2515.440 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2498.698 us, Write time : 1.623 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409381.168692727] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409381.168745437] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409382.168822073] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409382.168895964] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409383.168618988] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409383.168672269] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409384.168544386] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409384.168596786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409385.168642101] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409385.168699821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409386.168571648] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409386.168619880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409386.679756751] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.168627614] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.168678641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.230751648] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409387.803747322] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.803810622] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.926421709] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409387.926474179] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409388.037711899] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409388.168609043] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409388.168654981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409389.168650141] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409389.168705757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409390.168666223] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409390.168716829] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409391.168646240] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409391.168698810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409392.168828582] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409392.168884198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409393.168634648] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409393.168679894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409394.168718513] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409394.168769920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409395.168949629] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.169004944] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.909941197] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.340782 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409395.919957824] [controller_manager]: Overrun might occur, Total time : 2150.812 us (Expected < 2000.000 us) --> Read time : 17.423 us, Update time : 2131.726 us, Write time : 1.663 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409396.168922391] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409396.168985060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409397.168768905] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409397.168801838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409398.168825594] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409398.168885929] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409399.168617544] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409399.168670605] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409400.168685664] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409400.168740388] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409401.168576729] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.168627035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.864145748] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.545294 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409401.889363419] [controller_manager]: Overrun might occur, Total time : 3696.935 us (Expected < 2000.000 us) --> Read time : 15.209 us, Update time : 3680.103 us, Write time : 1.623 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409402.168785782] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409402.168838933] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.042217666] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.617312 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409403.168754002] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409403.168806141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409404.168930458] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409404.168988258] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409405.168656819] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.168713637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409405.883741647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141243 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409406.168748224] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409406.168792007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409407.168758304] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409407.168808890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409408.169015584] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409408.169073825] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409409.168998457] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409409.169058661] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409410.169063127] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409410.169117911] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409411.168889058] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409411.168949953] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409412.168682707] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409412.168735277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409413.168686195] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409413.168734176] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409414.168707079] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409414.168761112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409415.168682151] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409415.168731655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409416.168723509] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409416.168773504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409417.168728431] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409417.168788786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409418.168662024] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409418.168712651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409418.692097599] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409419.168590863] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.168647822] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.243053154] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409419.974630912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409419.974689082] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.132457961] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.132524468] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.168688443] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409420.168754629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409420.323711649] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409421.168665855] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409421.168718786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409422.168693455] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409422.168744622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409423.168757620] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409423.168808457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409424.168620052] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409424.168671309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409425.168679847] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409425.168725043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409426.168943416] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409426.168999473] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409427.168871052] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409427.168923962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409428.168735627] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409428.168787376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.070234811] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.634557 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.094189689] [controller_manager]: Overrun might occur, Total time : 4528.395 us (Expected < 2000.000 us) --> Read time : 13.746 us, Update time : 4512.875 us, Write time : 1.774 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409429.171402632] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409429.171751965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409430.168703576] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409430.168756106] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409431.169039363] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409431.169092334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409432.168863658] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409432.168908713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409433.168529702] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409433.168572122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409434.168659186] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.168714611] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409434.992257803] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.657528 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409435.090833216] [controller_manager]: Overrun might occur, Total time : 3161.207 us (Expected < 2000.000 us) --> Read time : 14.017 us, Update time : 3144.875 us, Write time : 2.315 us (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409435.168741616] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409435.168792142] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.162010756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.410522 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409436.170519778] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409436.172836610] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409437.168799060] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409437.168869524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409438.168667047] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409438.168718144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.008134935] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.534681 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409439.170499011] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409439.170532555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409440.168736591] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409440.168785474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409441.168805806] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409441.168850361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409442.168579248] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409442.168651325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409443.168638419] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409443.168682993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409444.168792460] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409444.168846292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409445.168762572] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409445.168820162] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409446.168688079] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409446.168757140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409447.168780098] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409447.168834010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409448.168510339] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409448.168563170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409449.168619830] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409449.168666699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.168730228] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409450.168786024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [INFO] [1778409450.461008411] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [ros2_control_node-1] [WARN] [1778409451.012029797] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [1778409328.626331243] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] [INFO] [1778409365.249856828] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:37:42 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] * Running on http://127.0.0.1:55479 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] * Running on http://172.30.43.138:55479 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:34] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:38 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.947805416] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.947870740] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.947886250] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.954234687] [moveit_2862854068.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.956083179] [moveit_2862854068.moveit.ros.rdf_loader]: Loaded robot model in 0.00169881 seconds (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.956119037] [moveit_2862854068.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.956142101] [moveit_2862854068.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409338.969190801] [moveit_2862854068.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409338.972569200] [moveit_2862854068.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.028067814] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.028206107] [moveit_2862854068.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029004604] [moveit_2862854068.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029469507] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029483203] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.029666977] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030101002] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030209608] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030926316] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.030940914] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.031468777] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.031991991] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409339.032301409] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409339.032320736] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.086094973] [moveit_2862854068.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.086999963] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088387885] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088402984] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088720813] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088733968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088754166] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088757783] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.088771980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.089527726] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.092121140] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.092135236] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.094831213] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.094848195] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.095581669] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.127577956] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.131480913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.131638001] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.131662277] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409339.132543862] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:40] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  Generated 20 grasp options for object Sphere1. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  IK accepted: 20 grasp options. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634391847] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634487779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634497508] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634513448] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409343.634582048] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.634983040] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.635071508] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.649743456] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.650238176] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.650473904] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:43 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653099818] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653130857] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653160413] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653169821] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.653188156] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654084109] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654116651] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654150005] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.654273108] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.655061171] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409343.655079866] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409345.155802840] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409345.161998774] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:46 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572016536] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572069947] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572077562] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572086438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409346.572152504] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572399634] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409346.572442024] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.576006642] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.576669040] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.577185752] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.577275187] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409356.577392325] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004906 seconds (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.586633244] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409356.586694701] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468429923] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468494736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468537698] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.468553548] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409358.468680920] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.469034803] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.469089697] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.481595403] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.481675145] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.481798158] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:58 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527465432] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527497793] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527516499] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527523913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.527544612] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528028391] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528070712] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528098304] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528217441] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528874800] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.528887825] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.779388726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409358.783990746] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:35:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:00 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004290652] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004355265] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004367528] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004385041] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409360.004467758] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004754443] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409360.004809999] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.008203543] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.008542507] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.012544340] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.013201114] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409370.013338494] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.008463 seconds (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.016164624] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [ERROR] [1778409370.016223867] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129566204] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129617511] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129626218] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129636527] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409371.129710939] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129946907] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.129996291] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.147975500] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148105236] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148245793] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148915560] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148946670] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148966267] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148973751] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.148992897] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150020636] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150072485] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150092112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.150235985] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.151109765] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.151124864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.601727028] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409371.607956204] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843686136] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843762842] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843775446] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.843788110] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409387.843891847] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.844153975] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.844202167] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.858503170] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.858580477] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.858662933] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:27 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924176112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924204516] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924221548] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924229974] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.924248880] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926018613] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926048620] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926066424] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926198525] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926765428] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409387.926777140] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409388.077129527] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409388.081924032] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:43 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:36:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081911888] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081972363] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081981300] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.081991981] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409420.082070380] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.082340934] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.082425179] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.094868617] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.094936957] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.095017520] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130348809] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130375690] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130392973] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130400297] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.130430554] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132017229] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132057475] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132083144] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132201449] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132724573] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.132738399] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.333148871] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409420.337930997] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:15 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:30 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666748406] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666823369] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666836894] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.666851522] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [WARN] [1778409451.666934129] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.667204322] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.667256652] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.680041173] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.680161080] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.680298401] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:31 INFO  Executing plan (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681315515] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681336776] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681350342] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681357034] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.681371913] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682027677] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682058265] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682076460] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682211777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682914292] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.682946714] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.833686322] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409451.839946267] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409462.270819172] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Exception in thread Thread-1 (spin): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self.run() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/threading.py", line 1010, in run (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self._target(*self._args, **self._kwargs) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self.spin_once() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self._spin_once_impl(timeout_sec) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] handler, entity, node = self.wait_for_ready_callbacks( (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] return next(self._cb_iter) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] wait_set = _rclpy.WaitSet( (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:37:46 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:29 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:29 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] conn.send({"status": "ok", "result": result}) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self._send_bytes(_ForkingPickler.dumps(obj)) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self._send(header + buf) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] n = write(self._handle, buf) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] BrokenPipeError: [Errno 32] Broken pipe (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:44 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:44 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] 2026-05-10 12:38:44 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:38:48 [ ERROR] [INFO] [1778409525.070796341] [moveit_2862854068.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -____________________ test_pick_and_place_sphere_by_id[ur5e] ____________________ - -self = -conn = -method = 'PUT', url = '/graspable/pick_up_object_by_id' -body = b'{"objectId":"Sphere1","effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' -headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '111'} -retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) -timeout = Timeout(connect=3.05, read=120, total=None), chunked = False -response_conn = -preload_content = False, decode_content = False, enforce_content_length = True - - def _make_request( - self, - conn: BaseHTTPConnection, - method: str, - url: str, - body: _TYPE_BODY | None = None, - headers: typing.Mapping[str, str] | None = None, - retries: Retry | None = None, - timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, - chunked: bool = False, - response_conn: BaseHTTPConnection | None = None, - preload_content: bool = True, - decode_content: bool = True, - enforce_content_length: bool = True, - ) -> BaseHTTPResponse: -  """ -  Perform a request on a given urllib connection object taken from our -  pool. -  -  :param conn: -  a connection from one of our connection pools -  -  :param method: -  HTTP request method (such as GET, POST, PUT, etc.) -  -  :param url: -  The URL to perform the request on. -  -  :param body: -  Data to send in the request body, either :class:`str`, :class:`bytes`, -  an iterable of :class:`str`/:class:`bytes`, or a file-like object. -  -  :param headers: -  Dictionary of custom headers to send, such as User-Agent, -  If-None-Match, etc. If None, pool headers are used. If provided, -  these headers completely replace any pool-specific headers. -  -  :param retries: -  Configure the number of retries to allow before raising a -  :class:`~urllib3.exceptions.MaxRetryError` exception. -  -  Pass ``None`` to retry until you receive a response. Pass a -  :class:`~urllib3.util.retry.Retry` object for fine-grained control -  over different types of retries. -  Pass an integer number to retry connection errors that many times, -  but no other types of errors. Pass zero to never retry. -  -  If ``False``, then retries are disabled and any exception is raised -  immediately. Also, instead of raising a MaxRetryError on redirects, -  the redirect response will be returned. -  -  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. -  -  :param timeout: -  If specified, overrides the default timeout for this one -  request. It may be a float (in seconds) or an instance of -  :class:`urllib3.util.Timeout`. -  -  :param chunked: -  If True, urllib3 will send the body using chunked transfer -  encoding. Otherwise, urllib3 will send the body using the standard -  content-length form. Defaults to False. -  -  :param response_conn: -  Set this to ``None`` if you will handle releasing the connection or -  set the connection to have the response release it. -  -  :param preload_content: -  If True, the response's body will be preloaded during construction. -  -  :param decode_content: -  If True, will attempt to decode the body based on the -  'content-encoding' header. -  -  :param enforce_content_length: -  Enforce content length checking. Body returned by server must match -  value of Content-Length header, if present. Otherwise, raise error. -  """ - self.num_requests += 1 -  - timeout_obj = self._get_timeout(timeout) - timeout_obj.start_connect() - conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) -  - try: - # Trigger any extra validation we need to do. - try: - self._validate_conn(conn) - except (SocketTimeout, BaseSSLError) as e: - self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) - raise -  - # _validate_conn() starts the connection to an HTTPS proxy - # so we need to wrap errors with 'ProxyError' here too. - except ( - OSError, - NewConnectionError, - TimeoutError, - BaseSSLError, - CertificateError, - SSLError, - ) as e: - new_e: Exception = e - if isinstance(e, (BaseSSLError, CertificateError)): - new_e = SSLError(e) - # If the connection didn't successfully connect to it's proxy - # then there - if isinstance( - new_e, (OSError, NewConnectionError, TimeoutError, SSLError) - ) and (conn and conn.proxy and not conn.has_connected_to_proxy): - new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) - raise new_e -  - # conn.request() calls http.client.*.request, not the method in - # urllib3.request. It also calls makefile (recv) on the socket. - try: - conn.request( - method, - url, - body=body, - headers=headers, - chunked=chunked, - preload_content=preload_content, - decode_content=decode_content, - enforce_content_length=enforce_content_length, - ) -  - # We are swallowing BrokenPipeError (errno.EPIPE) since the server is - # legitimately able to close the connection after sending a valid response. - # With this behaviour, the received response is still readable. - except BrokenPipeError: - pass - except OSError as e: - # MacOS/Linux - # EPROTOTYPE and ECONNRESET are needed on macOS - # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ - # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. - if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: - raise -  - # Reset the timeout for the recv() on the socket - read_timeout = timeout_obj.read_timeout -  - if not conn.is_closed: - # In Python 3 socket.py will catch EAGAIN and return None when you - # try and read into the file pointer created by http.client, which - # instead raises a BadStatusLine exception. Instead of catching - # the exception and assuming all BadStatusLine exceptions are read - # timeouts, check for a zero timeout before making the request. - if read_timeout == 0: - raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={read_timeout})" - ) - conn.timeout = read_timeout -  - # Receive the response from the server - try: -> response = conn.getresponse() - ^^^^^^^^^^^^^^^^^^ - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse - httplib_response = super().getresponse() - ^^^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:1428: in getresponse - response.begin() -/usr/lib/python3.12/http/client.py:331: in begin - version, status, reason = self._read_status() - ^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:292: in _read_status - line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -b = - - def readinto(self, b): -  """Read up to len(b) bytes into the writable buffer *b* and return -  the number of bytes read. If the socket is non-blocking and no bytes -  are available, None is returned. -  -  If *b* is non-empty, a 0 return value indicates that the connection -  was shutdown at the other end. -  """ - self._checkClosed() - self._checkReadable() - if self._timeout_occurred: - raise OSError("cannot read from timed out object") - while True: - try: -> return self._sock.recv_into(b) - ^^^^^^^^^^^^^^^^^^^^^^^ -E TimeoutError: timed out - -/usr/lib/python3.12/socket.py:707: TimeoutError - -The above exception was the direct cause of the following exception: - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: -> resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen - retries = retries.increment( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment - raise reraise(type(error), error, _stacktrace) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise - raise value -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen - response = self._make_request( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request - self._raise_timeout(err=e, url=url, timeout_value=read_timeout) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_id' -timeout_value = 120 - - def _raise_timeout( - self, - err: BaseSSLError | OSError | SocketTimeout, - url: str, - timeout_value: _TYPE_TIMEOUT | None, - ) -> None: -  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" -  - if isinstance(err, SocketTimeout): -> raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={timeout_value})" - ) from err -E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=55479): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError - -During handling of the above exception, another exception occurred: - -method = )> -url = 'http://0.0.0.0:55479/graspable/pick_up_object_by_id' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: -> resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - -src/python/arcor2_web/rest.py:259: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put - return request("put", url, data=data, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request - return session.request(method=method, url=url, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request - resp = self.send(prep, **send_kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send - r = adapter.send(request, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: - resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) -  - except (ProtocolError, OSError) as err: - raise ConnectionError(err, request=request) -  - except MaxRetryError as e: - if isinstance(e.reason, ConnectTimeoutError): - # TODO: Remove this in 3.0.0: see #2811 - if not isinstance(e.reason, NewConnectionError): - raise ConnectTimeout(e, request=request) -  - if isinstance(e.reason, ResponseError): - raise RetryError(e, request=request) -  - if isinstance(e.reason, _ProxyError): - raise ProxyError(e, request=request) -  - if isinstance(e.reason, _SSLError): - # This branch is for urllib3 v1.22 and later. - raise SSLError(e, request=request) -  - raise ConnectionError(e, request=request) -  - except ClosedPoolError as e: - raise ConnectionError(e, request=request) -  - except _ProxyError as e: - raise ProxyError(e) -  - except (_SSLError, _HTTPError) as e: - if isinstance(e, _SSLError): - # This branch is for urllib3 versions earlier than v1.22 - raise SSLError(e, request=request) - elif isinstance(e, ReadTimeoutError): -> raise ReadTimeout(e, request=request) -E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=55479): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout - -The above exception was the direct cause of the following exception: - -start_processes = Urls(ros_domain_id='34', robot_url='http://0.0.0.0:55479', storage_url='http://127.0.0.1:36283') - - @pytest.mark.timeout(321) - def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Sphere("Sphere1", 0.1) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id - rest.call( -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -method = )> -url = 'http://0.0.0.0:55479/graspable/pick_up_object_by_id' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: - resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - except requests.exceptions.RequestException as e: - logger.debug("Request failed.", exc_info=True) - # TODO would be good to provide more meaningful message but the original one could be very very long -> raise RestException("Catastrophic system error.") from e -E arcor2_web.rest.RestException: Catastrophic system error. - -src/python/arcor2_web/rest.py:269: RestException ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-35-25-644363-DESKTOP-HG3Q5KV-7301 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [7305] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [7306] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [7307] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [7308] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [7309] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409325.942210256] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.947799250] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.950736577] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.953197293] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.953231608] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409325.953242869] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409325.953350278] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.120336732] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.123018868] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.123324519] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.123385605] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.130833908] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132086789] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132119311] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132143697] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132148466] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132218880] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.132264066] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.219184727] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.471755962] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.471859349] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.475205731] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.501292831] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.502905389] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.503185982] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.503235807] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.503268659] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.509578615] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.516867314] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.516922709] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.518092171] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.531235480] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.531527279] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.532303705] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.534892644] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.534953159] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.535888250] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.549227422] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.549550246] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.550219006] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.553425880] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.553492536] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.554308937] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.568900458] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.569222675] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.569770275] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.572971619] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.573014150] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.577406760] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.590761463] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.591060816] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.601405402] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.601457040] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.601886996] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.615404792] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.615737239] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.621544348] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.621584614] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.622001422] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.636973710] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.637285186] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.641542316] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.641599645] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.642291695] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409326.659198425] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409326.659560593] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409326.781022611] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 7309] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.033381443] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.033434163] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.034814501] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.049050331] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.050262339] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.050331069] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.055115496] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.055939295] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.058943580] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.059809093] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.059851494] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.060220620] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.074733632] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.075025111] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.076317170] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.086903782] [controller_manager]: Overrun might occur, Total time : 9174.573 us (Expected < 2000.000 us) --> Read time : 13.265 us, Update time : 9159.004 us (Switch time : 9112.685 us (Switch chained mode time : 0.381 us, perform mode change time : 10.590 us, Activation time : 9086.385 us, Deactivation time : 0.140 us)), Write time : 2.304 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409327.086970298] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.369513 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.086920683] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.088776741] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.089966326] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.089995522] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.090479456] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.103155146] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.104523251] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.104833771] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.104891340] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.106364179] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.107781928] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.110607070] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.111831933] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.111868602] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.112987494] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.130674112] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.130986585] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.133232552] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.134426440] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.135838925] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.138782207] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.140021839] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.140057276] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.140286040] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.154638114] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.154913282] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.158551080] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.159850925] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.163190305] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.164577321] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.164606987] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.164915623] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_7ku1d6om -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.178563660] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.195106183] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195644772] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195819314] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195839432] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.195850523] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.197441006] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.204700484] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409327.205932341] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409327.209285311] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 7308] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409328.446955673] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 7306] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409328.739644512] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409328.739702071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409330.678208112] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.607607 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409331.853933576] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.333152 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409334.644270826] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.670582 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409335.168492845] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409335.168542008] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409336.168772539] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409336.168826491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409337.168743000] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409337.168798535] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409338.168720250] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409338.168795212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.109685823] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.085299 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409339.168899071] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409339.168946251] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409340.168841029] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409340.168888549] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409341.168868701] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409341.168944595] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.125686004] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409342.168751679] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.168832081] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409342.676798513] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.168854544] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409343.168909579] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409343.259686463] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.259741808] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.654803816] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409343.654870663] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409344.168600452] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409344.168650137] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.125736596] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.168692507] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409345.168736170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409345.331307588] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409345.882331048] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409346.168565487] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409346.168623286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409346.461173069] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409346.461215430] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409347.168623127] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409347.168675997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409348.168598029] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409348.168652983] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409349.168618695] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409349.168677768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409350.168572337] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409350.168640396] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409351.168651363] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409351.168701368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409352.168680082] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409352.168736830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409353.168612348] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409353.168682622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409354.168627087] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409354.168701629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409355.168722153] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409355.168785623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409356.168614422] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409356.168682111] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409356.589691529] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409357.140678175] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409357.168566571] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409357.168615704] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409357.837277912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409357.837336223] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.168591348] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409358.168642565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.528516719] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.528591521] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.731702722] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409358.788355279] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409359.168687876] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409359.168743141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409359.339379391] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409359.959512833] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409359.959563079] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409360.168877187] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409360.168927893] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409361.168837946] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409361.168890947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409361.618218898] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.618494 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409361.714251653] [controller_manager]: Overrun might occur, Total time : 2590.437 us (Expected < 2000.000 us) --> Read time : 13.196 us, Update time : 2575.488 us, Write time : 1.753 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409362.168603671] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409362.168652564] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409363.168658992] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409363.168731670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409364.168563690] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409364.168614857] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409365.168670830] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409365.168725093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409366.168697603] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409366.168775061] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409367.168768120] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.168819247] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.478199889] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.599615 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409367.504896572] [controller_manager]: Overrun might occur, Total time : 3234.988 us (Expected < 2000.000 us) --> Read time : 13.957 us, Update time : 3219.458 us, Write time : 1.573 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409368.168910775] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.168961120] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.726165594] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.564969 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409368.768172530] [controller_manager]: Overrun might occur, Total time : 2510.060 us (Expected < 2000.000 us) --> Read time : 13.867 us, Update time : 2494.610 us, Write time : 1.583 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409369.169061715] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409369.169118292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409370.018611113] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409370.168804323] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409370.168855200] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409370.569586880] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.126530371] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.126580216] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.150843629] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.150901199] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.168786939] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.168861501] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.520114881] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.514126 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409371.551684010] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409371.910258652] [controller_manager]: Overrun might occur, Total time : 2469.109 us (Expected < 2000.000 us) --> Read time : 666.808 us, Update time : 1800.157 us, Write time : 2.144 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409372.168876708] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409372.168932374] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409373.168777523] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409373.168829060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409374.168775783] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409374.168819696] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409375.168705212] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409375.168763553] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409376.168666972] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409376.168718791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409377.168645199] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.168695625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409377.798227590] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.627356 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409378.168606515] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.168658534] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.912603861] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.003546 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409378.932849665] [controller_manager]: Overrun might occur, Total time : 3190.701 us (Expected < 2000.000 us) --> Read time : 13.475 us, Update time : 3175.673 us, Write time : 1.553 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409379.168561076] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409379.168616782] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409380.168611025] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.168664727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.758219147] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.618923 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409380.906390507] [controller_manager]: Overrun might occur, Total time : 2515.440 us (Expected < 2000.000 us) --> Read time : 15.119 us, Update time : 2498.698 us, Write time : 1.623 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409381.168692727] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409381.168745437] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409382.168822073] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409382.168895964] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409383.168618988] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409383.168672269] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409384.168544386] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409384.168596786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409385.168642101] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409385.168699821] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409386.168571648] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409386.168619880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409386.679756751] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.168627614] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.168678641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.230751648] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409387.803747322] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.803810622] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.926421709] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409387.926474179] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409388.037711899] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409388.168609043] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409388.168654981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409389.168650141] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409389.168705757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409390.168666223] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409390.168716829] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409391.168646240] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409391.168698810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409392.168828582] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409392.168884198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409393.168634648] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409393.168679894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409394.168718513] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409394.168769920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409395.168949629] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.169004944] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.909941197] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.340782 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409395.919957824] [controller_manager]: Overrun might occur, Total time : 2150.812 us (Expected < 2000.000 us) --> Read time : 17.423 us, Update time : 2131.726 us, Write time : 1.663 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409396.168922391] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409396.168985060] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409397.168768905] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409397.168801838] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409398.168825594] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409398.168885929] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409399.168617544] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409399.168670605] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409400.168685664] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409400.168740388] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409401.168576729] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.168627035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.864145748] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.545294 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409401.889363419] [controller_manager]: Overrun might occur, Total time : 3696.935 us (Expected < 2000.000 us) --> Read time : 15.209 us, Update time : 3680.103 us, Write time : 1.623 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409402.168785782] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409402.168838933] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.042217666] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.617312 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409403.168754002] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409403.168806141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409404.168930458] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409404.168988258] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409405.168656819] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.168713637] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409405.883741647] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.141243 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409406.168748224] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409406.168792007] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409407.168758304] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409407.168808890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409408.169015584] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409408.169073825] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409409.168998457] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409409.169058661] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409410.169063127] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409410.169117911] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409411.168889058] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409411.168949953] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409412.168682707] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409412.168735277] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409413.168686195] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409413.168734176] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409414.168707079] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409414.168761112] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409415.168682151] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409415.168731655] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409416.168723509] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409416.168773504] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409417.168728431] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409417.168788786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409418.168662024] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409418.168712651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409418.692097599] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409419.168590863] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.168647822] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.243053154] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409419.974630912] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409419.974689082] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.132457961] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.132524468] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.168688443] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409420.168754629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409420.323711649] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409421.168665855] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409421.168718786] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409422.168693455] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409422.168744622] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409423.168757620] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409423.168808457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409424.168620052] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409424.168671309] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409425.168679847] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409425.168725043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409426.168943416] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409426.168999473] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409427.168871052] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409427.168923962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409428.168735627] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409428.168787376] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.070234811] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.634557 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.094189689] [controller_manager]: Overrun might occur, Total time : 4528.395 us (Expected < 2000.000 us) --> Read time : 13.746 us, Update time : 4512.875 us, Write time : 1.774 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409429.171402632] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409429.171751965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409430.168703576] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409430.168756106] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409431.169039363] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409431.169092334] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409432.168863658] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409432.168908713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409433.168529702] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409433.168572122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409434.168659186] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.168714611] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409434.992257803] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.657528 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409435.090833216] [controller_manager]: Overrun might occur, Total time : 3161.207 us (Expected < 2000.000 us) --> Read time : 14.017 us, Update time : 3144.875 us, Write time : 2.315 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409435.168741616] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409435.168792142] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.162010756] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.410522 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409436.170519778] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409436.172836610] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409437.168799060] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409437.168869524] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409438.168667047] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409438.168718144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.008134935] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.534681 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409439.170499011] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409439.170532555] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409440.168736591] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409440.168785474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409441.168805806] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409441.168850361] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409442.168579248] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409442.168651325] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409443.168638419] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409443.168682993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409444.168792460] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409444.168846292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409445.168762572] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409445.168820162] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409446.168688079] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409446.168757140] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409447.168780098] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409447.168834010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409448.168510339] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409448.168563170] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409449.168619830] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409449.168666699] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.168730228] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409450.168786024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409450.461008411] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409451.012029797] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409328.626331243] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409365.249856828] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:33 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:55479 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:55479 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:34] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 WARNING  Service /io_and_status_controller/set_speed_slider not available, waiting again... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.947805416] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.947870740] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.947886250] [moveit_2862854068.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.954234687] [moveit_2862854068.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.956083179] [moveit_2862854068.moveit.ros.rdf_loader]: Loaded robot model in 0.00169881 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.956119037] [moveit_2862854068.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.956142101] [moveit_2862854068.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409338.969190801] [moveit_2862854068.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409338.972569200] [moveit_2862854068.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.028067814] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.028206107] [moveit_2862854068.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029004604] [moveit_2862854068.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029469507] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029483203] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.029666977] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030101002] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030209608] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030926316] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.030940914] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.031468777] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.031991991] [moveit_2862854068.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409339.032301409] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409339.032320736] [moveit_2862854068.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.086094973] [moveit_2862854068.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.086999963] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088387885] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088402984] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088720813] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088733968] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088754166] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088757783] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.088771980] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.089527726] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.092121140] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.092135236] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.094831213] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.094848195] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.095581669] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.127577956] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.131480913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.131638001] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.131662277] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409339.132543862] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:40] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:35:40] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Generated 20 grasp options for object Sphere1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  IK accepted: 20 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634391847] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634487779] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634497508] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634513448] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409343.634582048] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.634983040] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.635071508] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.649743456] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.650238176] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.650473904] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:43 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653099818] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653130857] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653160413] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653169821] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.653188156] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654084109] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654116651] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654150005] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.654273108] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.655061171] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409343.655079866] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409345.155802840] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409345.161998774] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:46 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572016536] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572069947] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572077562] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572086438] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409346.572152504] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572399634] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409346.572442024] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.576006642] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.576669040] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.577185752] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.577275187] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409356.577392325] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004906 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.586633244] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409356.586694701] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468429923] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468494736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468537698] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.468553548] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409358.468680920] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.469034803] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.469089697] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.481595403] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.481675145] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.481798158] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:58 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527465432] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527497793] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527516499] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527523913] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.527544612] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528028391] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528070712] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528098304] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528217441] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528874800] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.528887825] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.779388726] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409358.783990746] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:35:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:00 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004290652] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004355265] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004367528] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004385041] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409360.004467758] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004754443] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409360.004809999] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.008203543] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.008542507] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.012544340] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.013201114] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409370.013338494] [moveit_2862854068.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.008463 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.016164624] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409370.016223867] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129566204] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129617511] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129626218] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129636527] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409371.129710939] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129946907] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.129996291] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.147975500] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148105236] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148245793] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148915560] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148946670] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148966267] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148973751] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.148992897] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150020636] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150072485] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150092112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.150235985] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.151109765] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.151124864] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.601727028] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409371.607956204] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843686136] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843762842] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843775446] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.843788110] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409387.843891847] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.844153975] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.844202167] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.858503170] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.858580477] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.858662933] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:27 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924176112] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924204516] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924221548] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924229974] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.924248880] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926018613] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926048620] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926066424] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926198525] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926765428] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409387.926777140] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409388.077129527] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409388.081924032] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:43 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:36:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081911888] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081972363] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081981300] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.081991981] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409420.082070380] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.082340934] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.082425179] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.094868617] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.094936957] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.095017520] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130348809] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130375690] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130392973] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130400297] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.130430554] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132017229] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132057475] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132083144] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132201449] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132724573] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.132738399] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.333148871] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409420.337930997] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:15 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666748406] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666823369] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666836894] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.666851522] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409451.666934129] [moveit_2862854068.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.667204322] [moveit_2862854068.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.667256652] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.680041173] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.680161080] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.680298401] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:31 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681315515] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681336776] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681350342] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681357034] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.681371913] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682027677] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682058265] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682076460] [moveit_2862854068.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682211777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682914292] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.682946714] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.833686322] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409451.839946267] [moveit_2862854068.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409462.270819172] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Exception in thread Thread-1 (spin): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1073, in _bootstrap_inner -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.run() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/threading.py", line 1010, in run -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._target(*self._args, **self._kwargs) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 308, in spin -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.spin_once() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 911, in spin_once -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._spin_once_impl(timeout_sec) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 890, in _spin_once_impl -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 handler, entity, node = self.wait_for_ready_callbacks( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 793, in wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 return next(self._cb_iter) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 673, in _wait_for_ready_callbacks -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 wait_set = _rclpy.WaitSet( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to initialize wait set: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/wait.c:130 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:37:46 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:29 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:29 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 conn.send({"status": "ok", "result": result}) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send_bytes(_ForkingPickler.dumps(obj)) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send(header + buf) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 n = write(self._handle, buf) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 BrokenPipeError: [Errno 32] Broken pipe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409525.070796341] [moveit_2862854068.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_sphere_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py::test_pick_and_place_sphere_by_id[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. -=================== 1 failed, 1 warning in 203.25s (0:03:23) =================== - - -12:39:05.80 [INFO] Long running tasks: - 67.55s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 387.60s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:39:35.84 [INFO] Long running tasks: - 61.53s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 97.58s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 417.64s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:40:05.87 [INFO] Long running tasks: - 91.57s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 127.62s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 447.67s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:40:35.91 [INFO] Long running tasks: - 121.61s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 157.66s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 477.71s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:41:05.94 [INFO] Long running tasks: - 151.64s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - 187.69s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - 507.74s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:41:22.34 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests -12:41:22.35 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-z7me8l -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py::test_pick_and_place_cylinder_by_id[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:40:17 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-59-296349-DESKTOP-HG3Q5KV-9863 (testutils.py:33) - -2026-05-10 12:40:17 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [9867] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [9868] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [9869] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-4]: process started with pid [9870] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-5]: process started with pid [9871] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409479.562757922] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.566787349] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.569029579] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.571343711] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.571373017] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.571378487] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409479.571474700] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.739618903] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.742206677] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.742606897] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.742671760] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.750900670] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752253025] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752289614] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752316626] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752323439] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752384204] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409479.752431624] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409479.820239011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.072447737] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.072511608] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.074832744] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.088952584] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.089990035] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.090098161] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.097361146] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.098030037] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.100941340] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.101667373] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.101729952] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.106129733] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.116806379] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.117040043] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.118619264] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409480.128639493] [controller_manager]: Overrun might occur, Total time : 8797.336 us (Expected < 2000.000 us) --> Read time : 13.145 us, Update time : 8781.867 us (Switch time : 8742.521 us (Switch chained mode time : 0.341 us, perform mode change time : 9.668 us, Activation time : 8718.015 us, Deactivation time : 0.080 us)), Write time : 2.324 us (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409480.128698636] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.953060 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.128653274] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.130703374] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.131489949] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.131554311] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.131918814] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.141420579] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.143138514] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.143394644] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.143440542] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.146654789] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.147927808] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.150866017] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.151786405] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.151831751] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.152824427] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.169192559] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.169484363] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.171629549] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.174711486] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.175947965] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.178890020] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.179830492] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.179864928] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.180223399] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.192674592] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.192912730] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.196650262] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.197966945] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.200839482] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.201757065] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.201790899] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.202119584] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.212776975] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.225066856] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225355962] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225475459] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225490938] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.225509473] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.227369978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.234620430] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.236060901] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-4] [INFO] [1778409480.239039090] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.326539066] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 9870] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.578991900] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.579053597] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.579475258] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.603219325] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604705028] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604810649] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604837300] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.604850274] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.606033072] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.613206212] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.613243703] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.614633223] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.626779497] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.627046414] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.627858127] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.631371592] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.631407210] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.632135294] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.646880090] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.647212572] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.647893717] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.651252178] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.651288447] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.652135947] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.666977974] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.667301434] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.667803457] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.671499741] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.671556448] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.671954309] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.684727760] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.684979243] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.693070613] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.693122772] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.693444809] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.708621206] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.708857986] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.715519317] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.715564062] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.716014784] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.730746371] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.731051405] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.735537364] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.735553886] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.735882841] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [spawner-5] [INFO] [1778409480.750995908] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409480.751342562] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 9871] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [robot_state_publisher-2] [INFO] [1778409482.107213394] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9868] (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409482.391966653] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409482.392025345] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409484.330301179] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.556265 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409485.470242198] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.497234 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.298220101] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.475137 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409488.720124456] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409488.720171405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409489.720395432] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409489.720442231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409490.720355201] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409490.720399616] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409491.720391041] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409491.720443581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409492.720307764] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409492.720361286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409493.720417390] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409493.720470421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409494.720511422] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409494.720566136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409495.720411205] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409495.720467472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409496.720371880] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409496.720424169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409497.720383220] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409497.720436300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409498.674596944] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409498.720397306] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409498.720441811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409499.225602069] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409499.720305331] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409499.720354715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409500.720492378] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409500.720539949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409500.871464013] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409500.871509008] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409501.132842191] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409501.132917364] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409501.720280895] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409501.720334096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409502.617879268] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409502.720366622] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409502.720414843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409503.720250154] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409503.720305148] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409504.720431021] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409504.720480034] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409505.720388949] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409505.720441329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409506.720341403] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409506.720394664] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409507.525071651] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409507.720283241] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409507.720334949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409508.076086947] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409508.658494613] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409508.658547634] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409508.720429463] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409508.720485659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409509.720302581] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409509.720354530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409510.720313552] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409510.720363727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409511.720412740] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409511.720471993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409512.720448517] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409512.720498883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409513.720342691] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409513.720393267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409514.501844758] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.099423 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409514.526177313] [controller_manager]: Overrun might occur, Total time : 2369.903 us (Expected < 2000.000 us) --> Read time : 14.117 us, Update time : 2354.063 us, Write time : 1.723 us (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409514.720425403] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409514.720477612] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.720505274] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409515.720553105] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.720347835] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409516.720392149] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409517.720240542] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409517.720274887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409518.720389133] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409518.720441262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409518.822012840] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409519.373082213] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409519.720340350] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409519.720391457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.021969391] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.022028453] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.357891419] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.145292 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.410651269] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.410681216] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.720342529] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.720397002] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409520.741886530] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409521.520304427] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.559242 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409521.720464786] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409521.720521313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409522.720424134] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409522.720479830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409523.720361755] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409523.720406791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.334114784] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.369799 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409524.720339658] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.720393711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409525.720322599] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409525.720379096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409526.720479646] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409526.720534691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409527.720348135] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409527.720387489] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409528.720433335] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409528.720488861] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409529.720372399] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409529.720424648] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.720277497] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409530.720330728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409531.720278468] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409531.720329635] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.720303383] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409532.720359540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409533.720236245] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409533.720293434] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409534.720338738] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409534.720392581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409535.720269335] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409535.720342194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409536.720276228] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409536.720333997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409536.834883830] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409537.385860176] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409537.720255351] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409537.720316136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409538.213665969] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.213725212] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.248816855] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.248876098] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.720322943] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409538.720382056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409539.720357151] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409539.720418768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409540.187890606] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409540.720329118] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409540.720393921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409541.720310509] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409541.720365083] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409542.720325347] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409542.720383498] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409543.720406529] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409543.720466132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409544.720480870] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.720535163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.720548589] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409545.720601069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409546.720326829] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409546.720387053] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409547.720292670] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409547.720334259] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409548.720419115] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.720487796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409549.720436570] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409549.720509338] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409550.720270536] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409550.720332043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409551.720318204] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409551.720380232] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409552.720342944] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409552.720399651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409553.720248767] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409553.720301026] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409554.720377705] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409554.720432589] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409555.720358475] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409555.720417588] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409556.720299325] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409556.720361503] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409557.720377446] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409557.720428724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409558.720324087] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409558.720389412] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409559.720361902] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409559.720416155] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409560.720216905] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409560.720271037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.720310064] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409561.720365198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409562.720468849] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409562.720521799] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409563.720356843] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409563.720410144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409564.720274650] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409564.720328141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409565.720282908] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409565.720339806] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409566.720407301] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409566.720460472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409567.720415334] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409567.720467924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409568.720337780] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409568.720390830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409569.720269051] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409569.720322262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409570.354004687] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409570.720336967] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409570.720388385] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409570.904947122] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409571.705004811] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409571.705059645] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409571.720342049] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409571.720391613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.200788182] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.200835632] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.299887588] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.720415390] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409572.720468692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409573.720280886] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409573.720345619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409574.720313601] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409574.720353357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409575.720295788] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409575.720341264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409576.720392283] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409576.720452998] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409577.720315495] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409577.720355631] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.720288790] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409578.720343163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409579.720259421] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409579.720315347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409580.720300967] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409580.720353657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409581.720524012] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409581.720593283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409582.720241878] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409582.720302624] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409583.720245808] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409583.720283119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409584.720284460] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409584.720350836] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409585.720699541] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409585.720749566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409586.720348968] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409586.720410415] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409587.720417664] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409587.720477027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409588.720196842] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409588.720247969] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409589.720368133] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409589.720409532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409590.720279357] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409590.720338490] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409591.720207477] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409591.720264565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.720470829] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409592.720524421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409593.720557875] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409593.720617368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409594.720483105] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409594.720539582] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409595.720454843] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409595.720508735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409596.720546614] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409596.720599093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409597.720530134] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409597.720592502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409598.720311891] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409598.720367126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409599.720281896] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409599.720343343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409600.720468456] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409600.720503452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409601.720290564] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409601.720350638] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409602.460351145] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409602.720270151] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409602.720330957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.011341711] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.587578600] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.587632111] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.720419041] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.720476110] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.866640870] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.866702627] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409604.035896384] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409604.720176040] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409604.720228119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409605.165782039] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.036464 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409605.720306450] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409605.720366985] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409606.720258617] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409606.720360099] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409607.720299035] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409607.720352757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409608.720209235] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409608.720268378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409609.720195281] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409609.720252961] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.720276347] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409610.720333346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.720292480] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409611.720355510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409612.720312726] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409612.720371307] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409613.720448590] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409613.720528292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409614.720359002] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409614.720421381] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409615.720386977] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409615.720456890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [INFO] [1778409616.720238325] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [ros2_control_node-1] [WARN] [1778409616.720292839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [1778409482.274302533] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] [INFO] [1778409518.151077238] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:40:17 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] * Running on http://127.0.0.1:46391 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] * Running on http://172.30.43.138:46391 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:08] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.897348835] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.897403238] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.897422816] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.903276087] [moveit_2981870913.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.904958734] [moveit_2981870913.moveit.ros.rdf_loader]: Loaded robot model in 0.001554 seconds (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.904984883] [moveit_2981870913.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.904993530] [moveit_2981870913.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409490.915268689] [moveit_2981870913.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.917684139] [moveit_2981870913.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.968975770] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.969101980] [moveit_2981870913.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970073354] [moveit_2981870913.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970635803] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970652144] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.970778704] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971169737] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971238047] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971977833] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.971992341] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.972503121] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409490.972974377] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409490.973210576] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409490.973221947] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.018992918] [moveit_2981870913.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.019647231] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.020733565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.020745698] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021036080] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021049736] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021076066] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021084622] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021092768] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.021656699] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.023827905] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.023842713] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.025375871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.025392673] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.026338791] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.060652420] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.064083168] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.064241038] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.064271276] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409491.065021531] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:16] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Generated 20 grasp options for object Cyl1. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  IK accepted: 20 grasp options. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112724876] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112858089] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112887144] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.112918914] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409501.112995971] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.113459802] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.113556105] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.129225328] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.129690542] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.129903537] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131176746] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131196554] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131230368] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131244044] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.131262349] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132154830] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132181731] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132208762] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.132328851] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.133141901] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409501.133163392] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409502.633798922] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409502.638179635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:28 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723005743] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723051380] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723058614] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723068202] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409508.723136933] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723359185] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409508.723465367] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.728265709] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.729542155] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.731513947] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.733111673] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409518.733255350] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.009719 seconds (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.752298446] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [ERROR] [1778409518.752363479] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386466390] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386526123] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386535060] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386546432] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409520.386629740] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386830692] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.386875327] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.403242471] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.403365846] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.403534386] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405519588] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405540918] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405558161] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405567098] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.405586635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410153317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410187132] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410210456] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.410326216] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.412025119] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.412035750] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.761397038] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409520.766119069] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] 2026-05-10 12:38:58 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219042785] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219111646] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219121475] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219132085] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [WARN] [1778409538.219201206] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219442986] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:21 [ ERROR] [INFO] [1778409538.219486328] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.244343716] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.245151571] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.245736012] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:38:58 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246576824] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246602343] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246625316] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246633772] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.246655494] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248162236] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248193065] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248210919] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.248354091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.249128953] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409538.249141767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:38:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:38:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409540.199555801] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409540.204121502] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:15 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:30 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:31 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182807387] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182895745] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182909761] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.182921854] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [WARN] [1778409572.182980426] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.183232274] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.183275216] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.196634030] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.196702079] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.196759829] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199676825] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199702755] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199720739] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199730788] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.199751317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200247800] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200284240] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200309057] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.200492185] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.201081339] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.201093933] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.301518928] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409572.306119871] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:32 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:33 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:33 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:34 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:34 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:47 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:39:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790751605] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790826097] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790836116] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.790847287] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [WARN] [1778409603.790905768] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.791185991] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.791242408] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.804213575] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.804277416] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.804406882] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:03 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864159500] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864183025] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864197252] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864204626] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.864221187] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866181662] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866214164] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866237728] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866354290] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866923035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409603.866934767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409604.067436224] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409604.072103202] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:13 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:13 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409617.594499160] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:40:19 WARNING  Transform did not update after motion, checking pose anyway. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:04 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:19 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:19 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] 2026-05-10 12:41:19 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:41:22 [ ERROR] [INFO] [1778409679.412950789] [moveit_2981870913.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -___________________ test_pick_and_place_cylinder_by_id[ur5e] ___________________ - -self = -conn = -method = 'PUT', url = '/graspable/pick_up_object_by_id' -body = b'{"objectId":"Cyl1","effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' -headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '108'} -retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) -timeout = Timeout(connect=3.05, read=120, total=None), chunked = False -response_conn = -preload_content = False, decode_content = False, enforce_content_length = True - - def _make_request( - self, - conn: BaseHTTPConnection, - method: str, - url: str, - body: _TYPE_BODY | None = None, - headers: typing.Mapping[str, str] | None = None, - retries: Retry | None = None, - timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, - chunked: bool = False, - response_conn: BaseHTTPConnection | None = None, - preload_content: bool = True, - decode_content: bool = True, - enforce_content_length: bool = True, - ) -> BaseHTTPResponse: -  """ -  Perform a request on a given urllib connection object taken from our -  pool. -  -  :param conn: -  a connection from one of our connection pools -  -  :param method: -  HTTP request method (such as GET, POST, PUT, etc.) -  -  :param url: -  The URL to perform the request on. -  -  :param body: -  Data to send in the request body, either :class:`str`, :class:`bytes`, -  an iterable of :class:`str`/:class:`bytes`, or a file-like object. -  -  :param headers: -  Dictionary of custom headers to send, such as User-Agent, -  If-None-Match, etc. If None, pool headers are used. If provided, -  these headers completely replace any pool-specific headers. -  -  :param retries: -  Configure the number of retries to allow before raising a -  :class:`~urllib3.exceptions.MaxRetryError` exception. -  -  Pass ``None`` to retry until you receive a response. Pass a -  :class:`~urllib3.util.retry.Retry` object for fine-grained control -  over different types of retries. -  Pass an integer number to retry connection errors that many times, -  but no other types of errors. Pass zero to never retry. -  -  If ``False``, then retries are disabled and any exception is raised -  immediately. Also, instead of raising a MaxRetryError on redirects, -  the redirect response will be returned. -  -  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. -  -  :param timeout: -  If specified, overrides the default timeout for this one -  request. It may be a float (in seconds) or an instance of -  :class:`urllib3.util.Timeout`. -  -  :param chunked: -  If True, urllib3 will send the body using chunked transfer -  encoding. Otherwise, urllib3 will send the body using the standard -  content-length form. Defaults to False. -  -  :param response_conn: -  Set this to ``None`` if you will handle releasing the connection or -  set the connection to have the response release it. -  -  :param preload_content: -  If True, the response's body will be preloaded during construction. -  -  :param decode_content: -  If True, will attempt to decode the body based on the -  'content-encoding' header. -  -  :param enforce_content_length: -  Enforce content length checking. Body returned by server must match -  value of Content-Length header, if present. Otherwise, raise error. -  """ - self.num_requests += 1 -  - timeout_obj = self._get_timeout(timeout) - timeout_obj.start_connect() - conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) -  - try: - # Trigger any extra validation we need to do. - try: - self._validate_conn(conn) - except (SocketTimeout, BaseSSLError) as e: - self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) - raise -  - # _validate_conn() starts the connection to an HTTPS proxy - # so we need to wrap errors with 'ProxyError' here too. - except ( - OSError, - NewConnectionError, - TimeoutError, - BaseSSLError, - CertificateError, - SSLError, - ) as e: - new_e: Exception = e - if isinstance(e, (BaseSSLError, CertificateError)): - new_e = SSLError(e) - # If the connection didn't successfully connect to it's proxy - # then there - if isinstance( - new_e, (OSError, NewConnectionError, TimeoutError, SSLError) - ) and (conn and conn.proxy and not conn.has_connected_to_proxy): - new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) - raise new_e -  - # conn.request() calls http.client.*.request, not the method in - # urllib3.request. It also calls makefile (recv) on the socket. - try: - conn.request( - method, - url, - body=body, - headers=headers, - chunked=chunked, - preload_content=preload_content, - decode_content=decode_content, - enforce_content_length=enforce_content_length, - ) -  - # We are swallowing BrokenPipeError (errno.EPIPE) since the server is - # legitimately able to close the connection after sending a valid response. - # With this behaviour, the received response is still readable. - except BrokenPipeError: - pass - except OSError as e: - # MacOS/Linux - # EPROTOTYPE and ECONNRESET are needed on macOS - # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ - # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. - if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: - raise -  - # Reset the timeout for the recv() on the socket - read_timeout = timeout_obj.read_timeout -  - if not conn.is_closed: - # In Python 3 socket.py will catch EAGAIN and return None when you - # try and read into the file pointer created by http.client, which - # instead raises a BadStatusLine exception. Instead of catching - # the exception and assuming all BadStatusLine exceptions are read - # timeouts, check for a zero timeout before making the request. - if read_timeout == 0: - raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={read_timeout})" - ) - conn.timeout = read_timeout -  - # Receive the response from the server - try: -> response = conn.getresponse() - ^^^^^^^^^^^^^^^^^^ - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse - httplib_response = super().getresponse() - ^^^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:1428: in getresponse - response.begin() -/usr/lib/python3.12/http/client.py:331: in begin - version, status, reason = self._read_status() - ^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:292: in _read_status - line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -b = - - def readinto(self, b): -  """Read up to len(b) bytes into the writable buffer *b* and return -  the number of bytes read. If the socket is non-blocking and no bytes -  are available, None is returned. -  -  If *b* is non-empty, a 0 return value indicates that the connection -  was shutdown at the other end. -  """ - self._checkClosed() - self._checkReadable() - if self._timeout_occurred: - raise OSError("cannot read from timed out object") - while True: - try: -> return self._sock.recv_into(b) - ^^^^^^^^^^^^^^^^^^^^^^^ -E TimeoutError: timed out - -/usr/lib/python3.12/socket.py:707: TimeoutError - -The above exception was the direct cause of the following exception: - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: -> resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen - retries = retries.increment( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment - raise reraise(type(error), error, _stacktrace) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise - raise value -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen - response = self._make_request( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request - self._raise_timeout(err=e, url=url, timeout_value=read_timeout) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_id' -timeout_value = 120 - - def _raise_timeout( - self, - err: BaseSSLError | OSError | SocketTimeout, - url: str, - timeout_value: _TYPE_TIMEOUT | None, - ) -> None: -  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" -  - if isinstance(err, SocketTimeout): -> raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={timeout_value})" - ) from err -E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=46391): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError - -During handling of the above exception, another exception occurred: - -method = )> -url = 'http://0.0.0.0:46391/graspable/pick_up_object_by_id' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: -> resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - -src/python/arcor2_web/rest.py:259: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put - return request("put", url, data=data, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request - return session.request(method=method, url=url, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request - resp = self.send(prep, **send_kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send - r = adapter.send(request, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: - resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) -  - except (ProtocolError, OSError) as err: - raise ConnectionError(err, request=request) -  - except MaxRetryError as e: - if isinstance(e.reason, ConnectTimeoutError): - # TODO: Remove this in 3.0.0: see #2811 - if not isinstance(e.reason, NewConnectionError): - raise ConnectTimeout(e, request=request) -  - if isinstance(e.reason, ResponseError): - raise RetryError(e, request=request) -  - if isinstance(e.reason, _ProxyError): - raise ProxyError(e, request=request) -  - if isinstance(e.reason, _SSLError): - # This branch is for urllib3 v1.22 and later. - raise SSLError(e, request=request) -  - raise ConnectionError(e, request=request) -  - except ClosedPoolError as e: - raise ConnectionError(e, request=request) -  - except _ProxyError as e: - raise ProxyError(e) -  - except (_SSLError, _HTTPError) as e: - if isinstance(e, _SSLError): - # This branch is for urllib3 versions earlier than v1.22 - raise SSLError(e, request=request) - elif isinstance(e, ReadTimeoutError): -> raise ReadTimeout(e, request=request) -E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=46391): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout - -The above exception was the direct cause of the following exception: - -start_processes = Urls(ros_domain_id='35', robot_url='http://0.0.0.0:46391', storage_url='http://127.0.0.1:52405') - - @pytest.mark.timeout(321) - def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Cylinder("Cyl1", 0.1, 0.2) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:240: in pick_up_object_by_id - rest.call( -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -method = )> -url = 'http://0.0.0.0:46391/graspable/pick_up_object_by_id' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: - resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - except requests.exceptions.RequestException as e: - logger.debug("Request failed.", exc_info=True) - # TODO would be good to provide more meaningful message but the original one could be very very long -> raise RestException("Catastrophic system error.") from e -E arcor2_web.rest.RestException: Catastrophic system error. - -src/python/arcor2_web/rest.py:269: RestException ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-37-59-296349-DESKTOP-HG3Q5KV-9863 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [9867] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [9868] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [9869] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [9870] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [9871] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409479.562757922] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.566787349] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.569029579] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.571343711] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.571373017] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.571378487] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409479.571474700] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.739618903] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.742206677] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.742606897] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.742671760] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.750900670] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752253025] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752289614] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752316626] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752323439] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752384204] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409479.752431624] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409479.820239011] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.072447737] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.072511608] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.074832744] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.088952584] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.089990035] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.090098161] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.097361146] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.098030037] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.100941340] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.101667373] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.101729952] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.106129733] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.116806379] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.117040043] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.118619264] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409480.128639493] [controller_manager]: Overrun might occur, Total time : 8797.336 us (Expected < 2000.000 us) --> Read time : 13.145 us, Update time : 8781.867 us (Switch time : 8742.521 us (Switch chained mode time : 0.341 us, perform mode change time : 9.668 us, Activation time : 8718.015 us, Deactivation time : 0.080 us)), Write time : 2.324 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409480.128698636] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.953060 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.128653274] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.130703374] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.131489949] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.131554311] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.131918814] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.141420579] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.143138514] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.143394644] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.143440542] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.146654789] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.147927808] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.150866017] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.151786405] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.151831751] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.152824427] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.169192559] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.169484363] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.171629549] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.174711486] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.175947965] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.178890020] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.179830492] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.179864928] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.180223399] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.192674592] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.192912730] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.196650262] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.197966945] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.200839482] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.201757065] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.201790899] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.202119584] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.212776975] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.225066856] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225355962] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225475459] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225490938] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.225509473] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.227369978] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.234620430] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.236060901] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409480.239039090] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.326539066] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 9870] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.578991900] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.579053597] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.579475258] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.603219325] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604705028] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604810649] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604837300] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.604850274] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.606033072] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.613206212] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.613243703] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.614633223] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.626779497] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.627046414] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.627858127] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.631371592] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.631407210] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.632135294] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.646880090] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.647212572] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.647893717] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.651252178] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.651288447] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.652135947] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.666977974] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.667301434] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.667803457] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.671499741] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.671556448] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.671954309] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.684727760] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.684979243] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.693070613] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.693122772] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.693444809] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.708621206] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.708857986] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.715519317] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.715564062] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.716014784] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.730746371] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.731051405] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.735537364] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.735553886] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.735882841] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_iqwwxrpi -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409480.750995908] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409480.751342562] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 9871] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409482.107213394] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 9868] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409482.391966653] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409482.392025345] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409484.330301179] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.556265 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409485.470242198] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.497234 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.298220101] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.475137 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409488.720124456] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409488.720171405] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409489.720395432] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409489.720442231] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409490.720355201] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409490.720399616] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409491.720391041] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409491.720443581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409492.720307764] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409492.720361286] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409493.720417390] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409493.720470421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409494.720511422] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409494.720566136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409495.720411205] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409495.720467472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409496.720371880] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409496.720424169] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409497.720383220] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409497.720436300] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409498.674596944] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409498.720397306] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409498.720441811] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409499.225602069] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409499.720305331] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409499.720354715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409500.720492378] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409500.720539949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409500.871464013] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409500.871509008] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409501.132842191] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409501.132917364] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409501.720280895] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409501.720334096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409502.617879268] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409502.720366622] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409502.720414843] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409503.720250154] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409503.720305148] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409504.720431021] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409504.720480034] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409505.720388949] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409505.720441329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409506.720341403] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409506.720394664] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409507.525071651] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409507.720283241] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409507.720334949] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409508.076086947] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409508.658494613] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409508.658547634] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409508.720429463] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409508.720485659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409509.720302581] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409509.720354530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409510.720313552] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409510.720363727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409511.720412740] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409511.720471993] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409512.720448517] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409512.720498883] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409513.720342691] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409513.720393267] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409514.501844758] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.099423 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409514.526177313] [controller_manager]: Overrun might occur, Total time : 2369.903 us (Expected < 2000.000 us) --> Read time : 14.117 us, Update time : 2354.063 us, Write time : 1.723 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409514.720425403] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409514.720477612] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.720505274] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409515.720553105] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.720347835] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409516.720392149] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409517.720240542] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409517.720274887] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409518.720389133] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409518.720441262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409518.822012840] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409519.373082213] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409519.720340350] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409519.720391457] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.021969391] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.022028453] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.357891419] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.145292 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.410651269] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.410681216] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.720342529] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.720397002] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409520.741886530] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409521.520304427] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.559242 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409521.720464786] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409521.720521313] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409522.720424134] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409522.720479830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409523.720361755] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409523.720406791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.334114784] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.369799 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409524.720339658] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.720393711] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409525.720322599] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409525.720379096] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409526.720479646] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409526.720534691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409527.720348135] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409527.720387489] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409528.720433335] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409528.720488861] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409529.720372399] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409529.720424648] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.720277497] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409530.720330728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409531.720278468] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409531.720329635] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.720303383] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409532.720359540] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409533.720236245] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409533.720293434] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409534.720338738] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409534.720392581] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409535.720269335] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409535.720342194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409536.720276228] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409536.720333997] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409536.834883830] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409537.385860176] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409537.720255351] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409537.720316136] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409538.213665969] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.213725212] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.248816855] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.248876098] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.720322943] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409538.720382056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409539.720357151] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409539.720418768] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409540.187890606] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409540.720329118] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409540.720393921] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409541.720310509] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409541.720365083] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409542.720325347] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409542.720383498] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409543.720406529] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409543.720466132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409544.720480870] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.720535163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.720548589] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409545.720601069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409546.720326829] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409546.720387053] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409547.720292670] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409547.720334259] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409548.720419115] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.720487796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409549.720436570] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409549.720509338] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409550.720270536] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409550.720332043] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409551.720318204] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409551.720380232] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409552.720342944] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409552.720399651] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409553.720248767] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409553.720301026] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409554.720377705] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409554.720432589] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409555.720358475] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409555.720417588] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409556.720299325] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409556.720361503] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409557.720377446] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409557.720428724] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409558.720324087] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409558.720389412] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409559.720361902] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409559.720416155] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409560.720216905] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409560.720271037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.720310064] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409561.720365198] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409562.720468849] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409562.720521799] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409563.720356843] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409563.720410144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409564.720274650] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409564.720328141] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409565.720282908] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409565.720339806] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409566.720407301] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409566.720460472] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409567.720415334] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409567.720467924] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409568.720337780] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409568.720390830] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409569.720269051] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409569.720322262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409570.354004687] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409570.720336967] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409570.720388385] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409570.904947122] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409571.705004811] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409571.705059645] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409571.720342049] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409571.720391613] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.200788182] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.200835632] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.299887588] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.720415390] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409572.720468692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409573.720280886] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409573.720345619] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409574.720313601] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409574.720353357] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409575.720295788] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409575.720341264] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409576.720392283] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409576.720452998] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409577.720315495] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409577.720355631] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.720288790] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409578.720343163] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409579.720259421] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409579.720315347] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409580.720300967] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409580.720353657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409581.720524012] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409581.720593283] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409582.720241878] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409582.720302624] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409583.720245808] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409583.720283119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409584.720284460] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409584.720350836] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409585.720699541] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409585.720749566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409586.720348968] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409586.720410415] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409587.720417664] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409587.720477027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409588.720196842] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409588.720247969] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409589.720368133] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409589.720409532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409590.720279357] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409590.720338490] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409591.720207477] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409591.720264565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.720470829] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409592.720524421] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409593.720557875] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409593.720617368] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409594.720483105] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409594.720539582] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409595.720454843] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409595.720508735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409596.720546614] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409596.720599093] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409597.720530134] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409597.720592502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409598.720311891] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409598.720367126] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409599.720281896] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409599.720343343] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409600.720468456] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409600.720503452] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409601.720290564] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409601.720350638] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409602.460351145] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409602.720270151] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409602.720330957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.011341711] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.587578600] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.587632111] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.720419041] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.720476110] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.866640870] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.866702627] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409604.035896384] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409604.720176040] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409604.720228119] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409605.165782039] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.036464 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409605.720306450] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409605.720366985] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409606.720258617] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409606.720360099] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409607.720299035] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409607.720352757] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409608.720209235] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409608.720268378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409609.720195281] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409609.720252961] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.720276347] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409610.720333346] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.720292480] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409611.720355510] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409612.720312726] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409612.720371307] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409613.720448590] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409613.720528292] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409614.720359002] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409614.720421381] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409615.720386977] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409615.720456890] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409616.720238325] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409616.720292839] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409482.274302533] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409518.151077238] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:07 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:46391 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:46391 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:08] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.897348835] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.897403238] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.897422816] [moveit_2981870913.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.903276087] [moveit_2981870913.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.904958734] [moveit_2981870913.moveit.ros.rdf_loader]: Loaded robot model in 0.001554 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.904984883] [moveit_2981870913.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.904993530] [moveit_2981870913.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409490.915268689] [moveit_2981870913.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.917684139] [moveit_2981870913.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.968975770] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.969101980] [moveit_2981870913.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970073354] [moveit_2981870913.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970635803] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970652144] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.970778704] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971169737] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971238047] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971977833] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.971992341] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.972503121] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409490.972974377] [moveit_2981870913.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409490.973210576] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409490.973221947] [moveit_2981870913.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.018992918] [moveit_2981870913.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.019647231] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.020733565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.020745698] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021036080] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021049736] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021076066] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021084622] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021092768] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.021656699] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.023827905] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.023842713] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.025375871] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.025392673] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.026338791] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.060652420] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.064083168] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.064241038] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.064271276] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409491.065021531] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:16] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:16] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Generated 20 grasp options for object Cyl1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  IK accepted: 20 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112724876] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112858089] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112887144] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.112918914] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409501.112995971] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.113459802] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.113556105] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.129225328] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.129690542] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.129903537] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131176746] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131196554] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131230368] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131244044] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.131262349] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132154830] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132181731] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132208762] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.132328851] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.133141901] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409501.133163392] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409502.633798922] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409502.638179635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:28 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723005743] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723051380] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723058614] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723068202] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409508.723136933] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723359185] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409508.723465367] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.728265709] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.729542155] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.731513947] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.733111673] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409518.733255350] [moveit_2981870913.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.009719 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.752298446] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409518.752363479] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386466390] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386526123] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386535060] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386546432] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409520.386629740] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386830692] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.386875327] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.403242471] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.403365846] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.403534386] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405519588] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405540918] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405558161] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405567098] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.405586635] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410153317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410187132] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410210456] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.410326216] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.412025119] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.412035750] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.761397038] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409520.766119069] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219042785] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219111646] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219121475] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219132085] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409538.219201206] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219442986] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.219486328] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.244343716] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.245151571] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.245736012] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246576824] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246602343] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246625316] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246633772] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.246655494] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248162236] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248193065] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248210919] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.248354091] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.249128953] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409538.249141767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409540.199555801] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409540.204121502] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:15 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:30 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:31 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182807387] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182895745] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182909761] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.182921854] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409572.182980426] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.183232274] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.183275216] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.196634030] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.196702079] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.196759829] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199676825] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199702755] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199720739] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199730788] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.199751317] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200247800] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200284240] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200309057] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.200492185] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.201081339] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.201093933] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.301518928] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409572.306119871] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:32 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:33 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:33 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:34 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:34 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:47 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790751605] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790826097] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790836116] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.790847287] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409603.790905768] [moveit_2981870913.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.791185991] [moveit_2981870913.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.791242408] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.804213575] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.804277416] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.804406882] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:03 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864159500] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864183025] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864197252] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864204626] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.864221187] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866181662] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866214164] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866237728] [moveit_2981870913.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866354290] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866923035] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409603.866934767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409604.067436224] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409604.072103202] [moveit_2981870913.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:13 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:13 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409617.594499160] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:19 WARNING  Transform did not update after motion, checking pose anyway. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:04 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:19 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:19 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:19 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409679.412950789] [moveit_2981870913.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_cylinder_by_id.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py::test_pick_and_place_cylinder_by_id[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. -=================== 1 failed, 1 warning in 203.26s (0:03:23) =================== - - -12:41:31.44 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests -12:41:31.45 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-S1w9dS -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py::test_pick_and_place_sphere_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:40:49 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-38-35-347899-DESKTOP-HG3Q5KV-10396 (testutils.py:33) - -2026-05-10 12:40:49 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [10403] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [10404] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [10405] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-4]: process started with pid [10406] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-5]: process started with pid [10407] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778409515.614720075] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.619154345] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.621406475] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.623664094] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.623692769] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.623698019] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409515.623791181] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.792608955] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.795312913] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.795735947] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.795814867] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.803864122] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805258311] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805294089] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805320689] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805327011] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805398477] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409515.805447941] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409515.868196952] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.120348299] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.120408774] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.122911189] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.135235717] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.136207639] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.136311366] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.143660720] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.144362038] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.147254494] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.148040543] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.148062915] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.152671783] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.163349327] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.163609335] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.164759470] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409516.175369977] [controller_manager]: Overrun might occur, Total time : 9190.454 us (Expected < 2000.000 us) --> Read time : 15.660 us, Update time : 9171.968 us (Switch time : 9091.265 us (Switch chained mode time : 0.350 us, perform mode change time : 13.836 us, Activation time : 9061.198 us, Deactivation time : 0.080 us)), Write time : 2.826 us (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409516.175421586] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.349235 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.175373894] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.177045361] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.177861617] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.177926530] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.178378719] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.188425494] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.191183986] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.191423050] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.191497802] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.192825444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.194319993] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.197267671] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.198233089] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.198280189] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.199245633] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.213014987] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.213264095] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.215392154] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.216944157] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.218302177] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.221433303] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.222533714] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.222575022] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.222846648] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.235104569] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.235343714] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.238847837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.240282088] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.243043875] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.243896505] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.243931903] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.244256955] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.254917437] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.267166806] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267418425] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267527542] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267542100] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.267558711] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.269226692] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.276952877] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.278382483] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-4] [INFO] [1778409516.281276106] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.380154622] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 10406] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.632501917] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.632553004] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.632959051] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.657232003] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658395588] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658516508] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658549000] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.658561914] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.659703920] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.665599106] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.665630485] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.666810828] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.681281194] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.681556833] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.682474947] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.685488327] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.685524946] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.686240521] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.699081171] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.699356043] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.699762706] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.701595372] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.701642372] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.702390398] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.717232571] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.717525236] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.718087374] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.721973524] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.722014822] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.722392034] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.735316282] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.735586341] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.743719188] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.743775104] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.744097888] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.759055439] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.759350254] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.765716551] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.765753751] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.766151948] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.779219845] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.779497812] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.783772545] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.783803133] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.784051925] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [spawner-5] [INFO] [1778409516.799237540] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409516.799530431] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 10407] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [robot_state_publisher-2] [INFO] [1778409518.151084992] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 10404] (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409518.439158296] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409518.439227378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409520.386323794] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.251694 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409521.520399618] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.327528 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.334067073] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.994833 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409524.773894503] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409524.773953906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409525.774256761] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409525.774314040] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409526.774306628] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409526.774358707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409527.774410363] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409527.774469125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409528.774233216] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409528.774290084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409529.121959026] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409529.672912769] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409529.774024087] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409529.774074012] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409530.345270410] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.345321968] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.605410877] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.605487803] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409530.773949089] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409530.774000967] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409531.774079476] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409531.774132056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.096202708] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.119024499] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409532.670080425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409532.774031337] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409532.774097132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409533.248410810] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409533.248467438] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409533.774180409] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409533.774243529] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409534.773855321] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409534.773902932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409535.774129847] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409535.774183599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409536.773930962] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409536.773985746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409537.774002583] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409537.774049703] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409538.774100402] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409538.774162981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409539.774002299] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409539.774056332] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409540.774175370] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409540.774226097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409541.773882336] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409541.773937781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409542.773921897] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409542.773970299] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409543.616000963] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409543.774225965] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409543.774283955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.166978678] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.730365547] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409544.730413689] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409544.774067229] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409544.774118987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.013145230] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.013212377] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.200213678] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409545.773905608] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409545.773960132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409546.774092834] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409546.774176894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409547.450396341] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409547.773981303] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409547.774040285] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.001355416] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.689603653] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409548.689655822] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409548.774057429] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409548.774118725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409549.773998729] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409549.774053152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409550.773967616] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409550.774009235] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409551.773924903] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409551.773978194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409552.773829558] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409552.773874965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409553.774031816] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409553.774090798] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409554.774103308] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409554.774169965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409555.773886465] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409555.773966747] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409556.773943660] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409556.773998254] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409557.773929952] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409557.773976100] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409558.773910633] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409558.773958805] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409558.803377498] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409559.354367550] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409559.773843524] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409559.773897948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409560.204243594] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409560.204303398] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409560.773902643] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409560.773958980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.267119864] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.267192923] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.530217000] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409561.773939050] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409561.774007270] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409562.773996808] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409562.774036885] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409563.774030510] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409563.774070516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409564.773924516] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409564.773970122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409565.774043119] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409565.774094577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409565.881504984] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409566.432644108] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409566.774034388] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409566.774082289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409567.001155661] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409567.001209824] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409567.773970499] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409567.774009714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409568.774047803] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409568.774107957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409569.773934862] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409569.773999314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409570.774038545] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409570.774128606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409571.773948667] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409571.774003121] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409572.773967184] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409572.774026367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409573.773927403] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409573.773975955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409574.774001829] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409574.774084987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409575.774054030] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409575.774111670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409576.773970368] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409576.774026785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409577.224523391] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409577.773795712] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409577.773844886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409577.775485538] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409578.381403603] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.381451193] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.523008149] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.523073833] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.770213408] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.773910918] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409578.773950603] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409578.869387265] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409579.420372240] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409579.773873084] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409579.773921035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409580.088215395] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409580.088265220] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409580.773981697] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409580.774029077] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409581.773926864] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409581.773994713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409582.774008467] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409582.774064303] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409583.773912381] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409583.773966153] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409584.773923530] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409584.773980728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409585.774172284] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409585.774224774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409586.773913606] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409586.773965645] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409587.774073967] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409587.774150071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409588.773970519] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409588.774011727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409589.773813822] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409589.773866962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409590.370976205] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409590.774007937] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409590.774056920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409590.922024831] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409591.774059297] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409591.774110494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409592.095135894] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.095182152] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.273238810] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.273303774] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.430231219] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409592.774015474] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409592.774065339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409593.774132524] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409593.774195364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409594.774019546] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409594.774070543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409595.774101603] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409595.774167358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409596.774127860] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409596.774199386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409597.390598637] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409597.774190341] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409597.774244915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409597.941608033] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409598.773942856] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409598.773990196] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409598.995449920] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409598.995503962] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409599.773922988] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409599.773972262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409600.774051254] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409600.774108633] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409601.774056185] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409601.774104718] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409602.773976855] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409602.774014907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409603.774356840] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409603.774419749] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409604.773930330] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409604.773983180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409605.773950419] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409605.774005474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409606.774020134] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409606.774066161] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409607.774048421] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409607.774106010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409608.774088678] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409608.774166386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409609.055960202] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409609.606941801] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409609.774169493] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409609.774225659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409610.352959853] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.353021380] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.603084781] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.603148121] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409610.773995161] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409610.774047620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.010219623] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.081872412] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409611.632848546] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409611.774113350] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409611.774177212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409612.555923196] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409612.556001194] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409612.773967665] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409612.774019694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409613.774180292] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409613.774235928] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409614.773978155] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409614.774031496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409615.774220848] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409615.774264561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409616.774007559] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409616.774065980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409617.773931531] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409617.773982808] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409618.773992303] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409618.774049221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409619.773918614] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409619.773971063] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409620.773903007] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409620.773952912] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409621.773889913] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409621.773940289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409622.622210318] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409622.773946168] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409622.774000221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409623.173189282] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409623.774036844] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409623.774092570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409623.934191606] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409623.934251310] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.237212707] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.237277891] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.396208081] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.745232323] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409624.774177762] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409624.774231314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409625.296182980] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409625.774109847] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409625.774199647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409626.092293549] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409626.092338926] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409626.773970851] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409626.774023932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409627.774030371] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409627.774082640] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409628.774080559] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409628.774172864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409629.774069059] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409629.774121389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409630.773954062] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409630.773998837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409631.774044663] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409631.774095069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409632.773973584] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409632.774025573] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409633.773996016] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409633.774048075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409634.773894930] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409634.773944574] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409635.774058799] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409635.774162656] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409636.226888766] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409636.773960831] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409636.774000817] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409636.777881425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409637.740621437] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.740680059] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.774168255] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409637.774217709] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.811214763] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409637.811277813] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409638.186218140] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409638.223759283] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [INFO] [1778409638.773919151] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [ros2_control_node-1] [WARN] [1778409638.773979115] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [1778409518.327323060] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] [INFO] [1778409649.065652015] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:40:49 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:43 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] * Running on http://127.0.0.1:40581 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] * Running on http://172.30.43.138:40581 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:44] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.036321843] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.036384503] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.036403499] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.043712542] [moveit_3371273600.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.046311475] [moveit_3371273600.moveit.ros.rdf_loader]: Loaded robot model in 0.00240338 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.046355829] [moveit_3371273600.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.046375286] [moveit_3371273600.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409527.058448036] [moveit_3371273600.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.061223344] [moveit_3371273600.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.123459179] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.123623521] [moveit_3371273600.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.124898750] [moveit_3371273600.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.125970481] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.125987874] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.126110713] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.126607397] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.126692659] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.127500915] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.127516565] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.128106269] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.128671513] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409527.128915657] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409527.128932279] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.300036403] [moveit_3371273600.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.301014070] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302496136] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302515343] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302869185] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302883713] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302913109] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302923148] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.302934670] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.303717588] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.306929731] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.306985452] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.309445206] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.309461647] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.310485471] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.344764911] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.348806181] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.348984670] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.349008385] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409527.349718264] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:47] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Generated 20 grasp options for object Sphere1. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  IK accepted: 20 grasp options. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440496294] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440600302] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440611423] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.440646389] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409530.440731441] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.441211763] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.441338995] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.446488090] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.446934037] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.447179864] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602618512] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602656344] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602686080] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602716979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.602742788] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604564483] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604608958] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604631030] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.604755596] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.605726145] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409530.605747786] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409532.106457668] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409532.112440632] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:38:53 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448804011] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448857072] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448864607] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.448874095] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409533.448938777] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.449175116] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409533.449219451] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.452772688] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.454306023] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.454424322] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.454508342] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409543.454607525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005345 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.458383809] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409543.458448913] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:04 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945762627] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945814846] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945821969] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.945833121] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409544.945897012] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.946172616] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.946219815] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.968519447] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.968632532] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409544.968723655] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:05 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011124691] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011170728] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011203200] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011216535] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.011240000] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012589945] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012630311] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012676599] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.012810694] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.013452719] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.013466795] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.214092836] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409545.218451112] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:08 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774712282] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774758810] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774766004] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.774775542] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409548.774863740] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.775097735] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409548.775141398] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.779581262] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.780121454] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.780268975] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.780419981] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409558.780535236] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005350 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.800944996] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409558.801046529] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:20 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348719480] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348776048] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348786568] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.348797649] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409560.348877580] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.349110393] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.349202087] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.354629520] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.354705074] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409560.354770558] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265629793] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265669729] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265692642] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265701880] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.265743740] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266610887] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266651835] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266673797] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.266810777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.267480865] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.267498929] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.568315361] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409561.574499051] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:27 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202704177] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202763680] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202774881] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.202786724] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409567.202859843] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.203090471] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409567.203141337] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.207691473] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.207891599] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.208002108] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.208108185] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409577.208265914] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005080 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.217294971] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409577.217359463] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505772198] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505845036] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505856238] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.505867399] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409578.505934126] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.506195122] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.506238875] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.519388381] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.519471308] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.519554106] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.521956721] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.521985205] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522008178] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522018698] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522037143] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522540840] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522574264] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522595304] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.522711555] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.523414977] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.523426589] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.773769996] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409578.778479835] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:40 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338541153] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338598071] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338606837] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338617979] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409580.338695666] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338927196] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409580.338977692] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.343040253] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.345094366] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.345230180] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.345319614] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409590.345504816] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.006463 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.364763815] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409590.364833277] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:52 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167800762] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167860736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167869933] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.167881415] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409592.167957028] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.168205651] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.168253432] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.181453949] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.181519112] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.181621507] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:52 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270644361] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270680600] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270700197] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270708032] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.270733801] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272575279] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272623882] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272650552] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.272792833] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.273506849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.273523220] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.474070690] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409592.478502712] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:53 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:56 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:56 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:39:59 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.040968481] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041020459] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041028465] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041041089] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409599.041110180] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041347050] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409599.041390182] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.045194942] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.045696525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.046111138] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.046183276] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409609.046352226] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004911 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.053176198] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409609.053236342] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579138144] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579189261] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579196304] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579205512] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409610.579276276] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579495833] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.579561318] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.593846621] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.593965648] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.594119229] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602094624] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602122527] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602190786] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602203390] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602221174] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602522210] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602557778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602579329] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.602717622] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.603342469] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409610.603354502] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409611.053987147] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409611.058476286] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:12 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607081664] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607132140] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607139153] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607148491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409612.607228623] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607434765] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409612.607528122] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612424804] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612692873] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612826407] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.612947938] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409622.613063829] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005436 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.620162557] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409622.620222320] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:23 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:24 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002605941] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002656427] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002665044] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002675924] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409624.002762188] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.002988879] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.003039686] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.021126594] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.021202598] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.021352833] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:24 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236059616] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236113979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236140249] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236149647] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236176688] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236600884] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236653975] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236677009] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.236797578] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.237505088] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.237518474] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.438057677] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409624.442508973] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:26 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204795781] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204854282] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204870944] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.204882556] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409626.204960574] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.205173719] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409626.205214186] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209519433] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209659123] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209800020] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.209923350] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409636.210041078] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004782 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.224498991] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409636.224555618] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769056818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769107575] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769114798] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769124096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409637.769201293] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769403146] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.769445507] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.808868039] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809009638] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809145216] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Executing plan (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809784746] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809806868] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809822778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809829661] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.809845552] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810603417] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810635037] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810651949] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.810762819] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.811477938] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409637.811489410] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:37 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409638.212176999] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409638.216435175] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:40:39 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.746963428] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747016419] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747023182] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747032860] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409639.747112772] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747319925] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409639.747394677] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409649.194918943] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.750215647] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.750931457] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.751485009] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.751625220] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [WARN] [1778409649.751723812] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004269 seconds (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.769075132] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [ERROR] [1778409649.769135707] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:13 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:13 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] conn.send({"status": "ok", "result": result}) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self._send_bytes(_ForkingPickler.dumps(obj)) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self._send(header + buf) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] n = write(self._handle, buf) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] BrokenPipeError: [Errno 32] Broken pipe (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:28 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:28 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] 2026-05-10 12:41:28 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:41:31 [ ERROR] [INFO] [1778409688.949668068] [moveit_3371273600.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -_________________ test_pick_and_place_sphere_by_position[ur5e] _________________ - -self = -conn = -method = 'PUT', url = '/graspable/pick_up_object_by_position' -body = b'{"position":{"x":0.0,"y":0.5,"z":0.1},"radius":0.5,"effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' -headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '140'} -retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) -timeout = Timeout(connect=3.05, read=120, total=None), chunked = False -response_conn = -preload_content = False, decode_content = False, enforce_content_length = True - - def _make_request( - self, - conn: BaseHTTPConnection, - method: str, - url: str, - body: _TYPE_BODY | None = None, - headers: typing.Mapping[str, str] | None = None, - retries: Retry | None = None, - timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, - chunked: bool = False, - response_conn: BaseHTTPConnection | None = None, - preload_content: bool = True, - decode_content: bool = True, - enforce_content_length: bool = True, - ) -> BaseHTTPResponse: -  """ -  Perform a request on a given urllib connection object taken from our -  pool. -  -  :param conn: -  a connection from one of our connection pools -  -  :param method: -  HTTP request method (such as GET, POST, PUT, etc.) -  -  :param url: -  The URL to perform the request on. -  -  :param body: -  Data to send in the request body, either :class:`str`, :class:`bytes`, -  an iterable of :class:`str`/:class:`bytes`, or a file-like object. -  -  :param headers: -  Dictionary of custom headers to send, such as User-Agent, -  If-None-Match, etc. If None, pool headers are used. If provided, -  these headers completely replace any pool-specific headers. -  -  :param retries: -  Configure the number of retries to allow before raising a -  :class:`~urllib3.exceptions.MaxRetryError` exception. -  -  Pass ``None`` to retry until you receive a response. Pass a -  :class:`~urllib3.util.retry.Retry` object for fine-grained control -  over different types of retries. -  Pass an integer number to retry connection errors that many times, -  but no other types of errors. Pass zero to never retry. -  -  If ``False``, then retries are disabled and any exception is raised -  immediately. Also, instead of raising a MaxRetryError on redirects, -  the redirect response will be returned. -  -  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. -  -  :param timeout: -  If specified, overrides the default timeout for this one -  request. It may be a float (in seconds) or an instance of -  :class:`urllib3.util.Timeout`. -  -  :param chunked: -  If True, urllib3 will send the body using chunked transfer -  encoding. Otherwise, urllib3 will send the body using the standard -  content-length form. Defaults to False. -  -  :param response_conn: -  Set this to ``None`` if you will handle releasing the connection or -  set the connection to have the response release it. -  -  :param preload_content: -  If True, the response's body will be preloaded during construction. -  -  :param decode_content: -  If True, will attempt to decode the body based on the -  'content-encoding' header. -  -  :param enforce_content_length: -  Enforce content length checking. Body returned by server must match -  value of Content-Length header, if present. Otherwise, raise error. -  """ - self.num_requests += 1 -  - timeout_obj = self._get_timeout(timeout) - timeout_obj.start_connect() - conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) -  - try: - # Trigger any extra validation we need to do. - try: - self._validate_conn(conn) - except (SocketTimeout, BaseSSLError) as e: - self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) - raise -  - # _validate_conn() starts the connection to an HTTPS proxy - # so we need to wrap errors with 'ProxyError' here too. - except ( - OSError, - NewConnectionError, - TimeoutError, - BaseSSLError, - CertificateError, - SSLError, - ) as e: - new_e: Exception = e - if isinstance(e, (BaseSSLError, CertificateError)): - new_e = SSLError(e) - # If the connection didn't successfully connect to it's proxy - # then there - if isinstance( - new_e, (OSError, NewConnectionError, TimeoutError, SSLError) - ) and (conn and conn.proxy and not conn.has_connected_to_proxy): - new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) - raise new_e -  - # conn.request() calls http.client.*.request, not the method in - # urllib3.request. It also calls makefile (recv) on the socket. - try: - conn.request( - method, - url, - body=body, - headers=headers, - chunked=chunked, - preload_content=preload_content, - decode_content=decode_content, - enforce_content_length=enforce_content_length, - ) -  - # We are swallowing BrokenPipeError (errno.EPIPE) since the server is - # legitimately able to close the connection after sending a valid response. - # With this behaviour, the received response is still readable. - except BrokenPipeError: - pass - except OSError as e: - # MacOS/Linux - # EPROTOTYPE and ECONNRESET are needed on macOS - # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ - # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. - if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: - raise -  - # Reset the timeout for the recv() on the socket - read_timeout = timeout_obj.read_timeout -  - if not conn.is_closed: - # In Python 3 socket.py will catch EAGAIN and return None when you - # try and read into the file pointer created by http.client, which - # instead raises a BadStatusLine exception. Instead of catching - # the exception and assuming all BadStatusLine exceptions are read - # timeouts, check for a zero timeout before making the request. - if read_timeout == 0: - raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={read_timeout})" - ) - conn.timeout = read_timeout -  - # Receive the response from the server - try: -> response = conn.getresponse() - ^^^^^^^^^^^^^^^^^^ - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse - httplib_response = super().getresponse() - ^^^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:1428: in getresponse - response.begin() -/usr/lib/python3.12/http/client.py:331: in begin - version, status, reason = self._read_status() - ^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:292: in _read_status - line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -b = - - def readinto(self, b): -  """Read up to len(b) bytes into the writable buffer *b* and return -  the number of bytes read. If the socket is non-blocking and no bytes -  are available, None is returned. -  -  If *b* is non-empty, a 0 return value indicates that the connection -  was shutdown at the other end. -  """ - self._checkClosed() - self._checkReadable() - if self._timeout_occurred: - raise OSError("cannot read from timed out object") - while True: - try: -> return self._sock.recv_into(b) - ^^^^^^^^^^^^^^^^^^^^^^^ -E TimeoutError: timed out - -/usr/lib/python3.12/socket.py:707: TimeoutError - -The above exception was the direct cause of the following exception: - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: -> resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen - retries = retries.increment( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment - raise reraise(type(error), error, _stacktrace) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise - raise value -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen - response = self._make_request( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request - self._raise_timeout(err=e, url=url, timeout_value=read_timeout) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_position' -timeout_value = 120 - - def _raise_timeout( - self, - err: BaseSSLError | OSError | SocketTimeout, - url: str, - timeout_value: _TYPE_TIMEOUT | None, - ) -> None: -  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" -  - if isinstance(err, SocketTimeout): -> raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={timeout_value})" - ) from err -E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=40581): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError - -During handling of the above exception, another exception occurred: - -method = )> -url = 'http://0.0.0.0:40581/graspable/pick_up_object_by_position' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: -> resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - -src/python/arcor2_web/rest.py:259: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put - return request("put", url, data=data, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request - return session.request(method=method, url=url, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request - resp = self.send(prep, **send_kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send - r = adapter.send(request, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: - resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) -  - except (ProtocolError, OSError) as err: - raise ConnectionError(err, request=request) -  - except MaxRetryError as e: - if isinstance(e.reason, ConnectTimeoutError): - # TODO: Remove this in 3.0.0: see #2811 - if not isinstance(e.reason, NewConnectionError): - raise ConnectTimeout(e, request=request) -  - if isinstance(e.reason, ResponseError): - raise RetryError(e, request=request) -  - if isinstance(e.reason, _ProxyError): - raise ProxyError(e, request=request) -  - if isinstance(e.reason, _SSLError): - # This branch is for urllib3 v1.22 and later. - raise SSLError(e, request=request) -  - raise ConnectionError(e, request=request) -  - except ClosedPoolError as e: - raise ConnectionError(e, request=request) -  - except _ProxyError as e: - raise ProxyError(e) -  - except (_SSLError, _HTTPError) as e: - if isinstance(e, _SSLError): - # This branch is for urllib3 versions earlier than v1.22 - raise SSLError(e, request=request) - elif isinstance(e, ReadTimeoutError): -> raise ReadTimeout(e, request=request) -E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=40581): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout - -The above exception was the direct cause of the following exception: - -start_processes = Urls(ros_domain_id='156', robot_url='http://0.0.0.0:40581', storage_url='http://127.0.0.1:36483') - - @pytest.mark.timeout(321) - def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - object = Sphere("Sphere1", 0.1) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position - rest.call( -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -method = )> -url = 'http://0.0.0.0:40581/graspable/pick_up_object_by_position' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: - resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - except requests.exceptions.RequestException as e: - logger.debug("Request failed.", exc_info=True) - # TODO would be good to provide more meaningful message but the original one could be very very long -> raise RestException("Catastrophic system error.") from e -E arcor2_web.rest.RestException: Catastrophic system error. - -src/python/arcor2_web/rest.py:269: RestException ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-38-35-347899-DESKTOP-HG3Q5KV-10396 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [10403] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [10404] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [10405] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [10406] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [10407] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409515.614720075] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.619154345] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.621406475] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.623664094] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.623692769] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.623698019] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409515.623791181] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.792608955] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.795312913] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.795735947] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.795814867] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.803864122] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805258311] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805294089] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805320689] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805327011] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805398477] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409515.805447941] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409515.868196952] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.120348299] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.120408774] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.122911189] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.135235717] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.136207639] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.136311366] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.143660720] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.144362038] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.147254494] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.148040543] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.148062915] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.152671783] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.163349327] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.163609335] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.164759470] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409516.175369977] [controller_manager]: Overrun might occur, Total time : 9190.454 us (Expected < 2000.000 us) --> Read time : 15.660 us, Update time : 9171.968 us (Switch time : 9091.265 us (Switch chained mode time : 0.350 us, perform mode change time : 13.836 us, Activation time : 9061.198 us, Deactivation time : 0.080 us)), Write time : 2.826 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409516.175421586] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 9.349235 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.175373894] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.177045361] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.177861617] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.177926530] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.178378719] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.188425494] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.191183986] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.191423050] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.191497802] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.192825444] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.194319993] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.197267671] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.198233089] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.198280189] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.199245633] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.213014987] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.213264095] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.215392154] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.216944157] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.218302177] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.221433303] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.222533714] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.222575022] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.222846648] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.235104569] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.235343714] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.238847837] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.240282088] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.243043875] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.243896505] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.243931903] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.244256955] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.254917437] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.267166806] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267418425] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267527542] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267542100] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.267558711] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.269226692] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.276952877] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.278382483] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409516.281276106] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.380154622] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 10406] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.632501917] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.632553004] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.632959051] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.657232003] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658395588] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658516508] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658549000] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.658561914] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.659703920] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.665599106] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.665630485] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.666810828] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.681281194] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.681556833] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.682474947] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.685488327] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.685524946] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.686240521] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.699081171] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.699356043] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.699762706] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.701595372] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.701642372] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.702390398] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.717232571] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.717525236] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.718087374] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.721973524] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.722014822] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.722392034] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.735316282] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.735586341] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.743719188] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.743775104] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.744097888] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.759055439] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.759350254] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.765716551] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.765753751] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.766151948] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.779219845] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.779497812] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.783772545] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.783803133] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.784051925] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xg2tbhrt -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409516.799237540] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409516.799530431] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 10407] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409518.151084992] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 10404] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409518.439158296] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409518.439227378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409520.386323794] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.251694 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409521.520399618] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.327528 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.334067073] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 3.994833 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409524.773894503] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409524.773953906] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409525.774256761] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409525.774314040] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409526.774306628] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409526.774358707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409527.774410363] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409527.774469125] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409528.774233216] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409528.774290084] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409529.121959026] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409529.672912769] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409529.774024087] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409529.774074012] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409530.345270410] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.345321968] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.605410877] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.605487803] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409530.773949089] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409530.774000967] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409531.774079476] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409531.774132056] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.096202708] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.119024499] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409532.670080425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409532.774031337] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409532.774097132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409533.248410810] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409533.248467438] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409533.774180409] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409533.774243529] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409534.773855321] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409534.773902932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409535.774129847] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409535.774183599] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409536.773930962] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409536.773985746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409537.774002583] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409537.774049703] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409538.774100402] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409538.774162981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409539.774002299] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409539.774056332] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409540.774175370] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409540.774226097] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409541.773882336] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409541.773937781] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409542.773921897] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409542.773970299] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409543.616000963] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409543.774225965] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409543.774283955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.166978678] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.730365547] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409544.730413689] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409544.774067229] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409544.774118987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.013145230] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.013212377] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.200213678] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409545.773905608] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409545.773960132] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409546.774092834] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409546.774176894] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409547.450396341] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409547.773981303] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409547.774040285] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.001355416] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.689603653] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409548.689655822] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409548.774057429] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409548.774118725] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409549.773998729] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409549.774053152] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409550.773967616] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409550.774009235] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409551.773924903] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409551.773978194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409552.773829558] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409552.773874965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409553.774031816] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409553.774090798] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409554.774103308] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409554.774169965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409555.773886465] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409555.773966747] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409556.773943660] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409556.773998254] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409557.773929952] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409557.773976100] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409558.773910633] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409558.773958805] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409558.803377498] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409559.354367550] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409559.773843524] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409559.773897948] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409560.204243594] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409560.204303398] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409560.773902643] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409560.773958980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.267119864] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.267192923] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.530217000] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409561.773939050] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409561.774007270] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409562.773996808] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409562.774036885] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409563.774030510] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409563.774070516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409564.773924516] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409564.773970122] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409565.774043119] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409565.774094577] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409565.881504984] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409566.432644108] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409566.774034388] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409566.774082289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409567.001155661] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409567.001209824] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409567.773970499] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409567.774009714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409568.774047803] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409568.774107957] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409569.773934862] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409569.773999314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409570.774038545] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409570.774128606] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409571.773948667] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409571.774003121] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409572.773967184] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409572.774026367] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409573.773927403] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409573.773975955] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409574.774001829] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409574.774084987] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409575.774054030] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409575.774111670] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409576.773970368] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409576.774026785] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409577.224523391] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409577.773795712] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409577.773844886] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409577.775485538] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409578.381403603] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.381451193] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.523008149] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.523073833] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.770213408] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.773910918] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409578.773950603] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409578.869387265] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409579.420372240] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409579.773873084] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409579.773921035] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409580.088215395] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409580.088265220] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409580.773981697] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409580.774029077] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409581.773926864] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409581.773994713] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409582.774008467] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409582.774064303] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409583.773912381] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409583.773966153] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409584.773923530] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409584.773980728] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409585.774172284] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409585.774224774] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409586.773913606] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409586.773965645] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409587.774073967] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409587.774150071] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409588.773970519] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409588.774011727] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409589.773813822] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409589.773866962] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409590.370976205] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409590.774007937] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409590.774056920] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409590.922024831] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409591.774059297] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409591.774110494] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409592.095135894] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.095182152] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.273238810] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.273303774] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.430231219] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409592.774015474] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409592.774065339] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409593.774132524] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409593.774195364] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409594.774019546] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409594.774070543] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409595.774101603] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409595.774167358] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409596.774127860] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409596.774199386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409597.390598637] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409597.774190341] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409597.774244915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409597.941608033] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409598.773942856] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409598.773990196] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409598.995449920] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409598.995503962] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409599.773922988] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409599.773972262] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409600.774051254] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409600.774108633] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409601.774056185] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409601.774104718] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409602.773976855] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409602.774014907] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409603.774356840] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409603.774419749] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409604.773930330] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409604.773983180] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409605.773950419] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409605.774005474] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409606.774020134] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409606.774066161] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409607.774048421] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409607.774106010] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409608.774088678] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409608.774166386] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409609.055960202] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409609.606941801] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409609.774169493] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409609.774225659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409610.352959853] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.353021380] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.603084781] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.603148121] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409610.773995161] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409610.774047620] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.010219623] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.081872412] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409611.632848546] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409611.774113350] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409611.774177212] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409612.555923196] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409612.556001194] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409612.773967665] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409612.774019694] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409613.774180292] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409613.774235928] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409614.773978155] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409614.774031496] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409615.774220848] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409615.774264561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409616.774007559] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409616.774065980] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409617.773931531] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409617.773982808] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409618.773992303] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409618.774049221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409619.773918614] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409619.773971063] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409620.773903007] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409620.773952912] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409621.773889913] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409621.773940289] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409622.622210318] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409622.773946168] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409622.774000221] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409623.173189282] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409623.774036844] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409623.774092570] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409623.934191606] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409623.934251310] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.237212707] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.237277891] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.396208081] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.745232323] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409624.774177762] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409624.774231314] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409625.296182980] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409625.774109847] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409625.774199647] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409626.092293549] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409626.092338926] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409626.773970851] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409626.774023932] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409627.774030371] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409627.774082640] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409628.774080559] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409628.774172864] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409629.774069059] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409629.774121389] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409630.773954062] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409630.773998837] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409631.774044663] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409631.774095069] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409632.773973584] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409632.774025573] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409633.773996016] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409633.774048075] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409634.773894930] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409634.773944574] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409635.774058799] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409635.774162656] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409636.226888766] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409636.773960831] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409636.774000817] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409636.777881425] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409637.740621437] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.740680059] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.774168255] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409637.774217709] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.811214763] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409637.811277813] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409638.186218140] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409638.223759283] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409638.773919151] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409638.773979115] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409518.327323060] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409649.065652015] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:43 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:40581 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:40581 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:44] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.036321843] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.036384503] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.036403499] [moveit_3371273600.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.043712542] [moveit_3371273600.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.046311475] [moveit_3371273600.moveit.ros.rdf_loader]: Loaded robot model in 0.00240338 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.046355829] [moveit_3371273600.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.046375286] [moveit_3371273600.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409527.058448036] [moveit_3371273600.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.061223344] [moveit_3371273600.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.123459179] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.123623521] [moveit_3371273600.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.124898750] [moveit_3371273600.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.125970481] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.125987874] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.126110713] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.126607397] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.126692659] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.127500915] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.127516565] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.128106269] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.128671513] [moveit_3371273600.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409527.128915657] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409527.128932279] [moveit_3371273600.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.300036403] [moveit_3371273600.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.301014070] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302496136] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302515343] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302869185] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302883713] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302913109] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302923148] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.302934670] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.303717588] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.306929731] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.306985452] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.309445206] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.309461647] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.310485471] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.344764911] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.348806181] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.348984670] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.349008385] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409527.349718264] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:47] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:38:47] "PUT /collisions/sphere?radius=0.1&sphereId=Sphere1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Generated 20 grasp options for object Sphere1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  IK accepted: 20 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440496294] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440600302] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440611423] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.440646389] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409530.440731441] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.441211763] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.441338995] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.446488090] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.446934037] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.447179864] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602618512] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602656344] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602686080] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602716979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.602742788] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604564483] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604608958] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604631030] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.604755596] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.605726145] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409530.605747786] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409532.106457668] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409532.112440632] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:38:53 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448804011] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448857072] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448864607] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.448874095] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409533.448938777] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.449175116] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409533.449219451] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.452772688] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.454306023] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.454424322] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.454508342] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409543.454607525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005345 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.458383809] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409543.458448913] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:04 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945762627] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945814846] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945821969] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.945833121] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409544.945897012] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.946172616] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.946219815] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.968519447] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.968632532] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409544.968723655] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011124691] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011170728] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011203200] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011216535] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.011240000] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012589945] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012630311] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012676599] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.012810694] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.013452719] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.013466795] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.214092836] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409545.218451112] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:08 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774712282] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774758810] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774766004] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.774775542] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409548.774863740] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.775097735] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409548.775141398] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.779581262] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.780121454] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.780268975] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.780419981] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409558.780535236] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005350 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.800944996] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409558.801046529] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:20 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348719480] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348776048] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348786568] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.348797649] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409560.348877580] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.349110393] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.349202087] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.354629520] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.354705074] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409560.354770558] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265629793] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265669729] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265692642] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265701880] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.265743740] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266610887] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266651835] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266673797] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.266810777] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.267480865] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.267498929] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.568315361] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409561.574499051] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:27 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202704177] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202763680] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202774881] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.202786724] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409567.202859843] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.203090471] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409567.203141337] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.207691473] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.207891599] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.208002108] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.208108185] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409577.208265914] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005080 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.217294971] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409577.217359463] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505772198] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505845036] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505856238] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.505867399] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409578.505934126] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.506195122] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.506238875] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.519388381] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.519471308] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.519554106] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.521956721] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.521985205] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522008178] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522018698] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522037143] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522540840] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522574264] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522595304] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.522711555] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.523414977] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.523426589] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.773769996] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409578.778479835] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:40 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338541153] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338598071] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338606837] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338617979] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409580.338695666] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338927196] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409580.338977692] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.343040253] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.345094366] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.345230180] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.345319614] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409590.345504816] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.006463 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.364763815] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409590.364833277] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167800762] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167860736] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167869933] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.167881415] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409592.167957028] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.168205651] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.168253432] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.181453949] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.181519112] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.181621507] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:52 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270644361] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270680600] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270700197] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270708032] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.270733801] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272575279] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272623882] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272650552] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.272792833] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.273506849] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.273523220] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.474070690] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409592.478502712] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:53 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:56 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:56 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:39:59 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.040968481] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041020459] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041028465] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041041089] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409599.041110180] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041347050] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409599.041390182] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.045194942] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.045696525] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.046111138] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.046183276] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409609.046352226] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004911 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.053176198] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409609.053236342] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579138144] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579189261] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579196304] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579205512] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409610.579276276] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579495833] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.579561318] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.593846621] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.593965648] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.594119229] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602094624] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602122527] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602190786] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602203390] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602221174] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602522210] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602557778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602579329] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.602717622] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.603342469] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409610.603354502] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409611.053987147] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409611.058476286] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:12 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607081664] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607132140] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607139153] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607148491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409612.607228623] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607434765] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409612.607528122] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612424804] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612692873] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612826407] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.612947938] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409622.613063829] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.005436 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.620162557] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409622.620222320] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:23 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:24 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002605941] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002656427] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002665044] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002675924] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409624.002762188] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.002988879] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.003039686] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.021126594] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.021202598] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.021352833] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:24 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236059616] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236113979] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236140249] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236149647] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236176688] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236600884] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236653975] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236677009] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.236797578] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.237505088] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.237518474] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.438057677] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409624.442508973] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:26 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204795781] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204854282] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204870944] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.204882556] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409626.204960574] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.205173719] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409626.205214186] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209519433] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209659123] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209800020] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.209923350] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409636.210041078] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004782 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.224498991] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409636.224555618] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769056818] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769107575] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769114798] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769124096] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409637.769201293] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769403146] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.769445507] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.808868039] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809009638] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809145216] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809784746] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809806868] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809822778] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809829661] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.809845552] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810603417] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810635037] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810651949] [moveit_3371273600.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.810762819] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.811477938] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409637.811489410] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:37 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409638.212176999] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409638.216435175] [moveit_3371273600.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:40:39 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.746963428] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747016419] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747023182] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747032860] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409639.747112772] [moveit_3371273600.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747319925] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409639.747394677] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409649.194918943] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.750215647] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.750931457] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.751485009] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.751625220] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409649.751723812] [moveit_3371273600.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004269 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.769075132] [moveit_3371273600.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409649.769135707] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:13 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Sphere1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:13 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 conn.send({"status": "ok", "result": result}) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send_bytes(_ForkingPickler.dumps(obj)) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send(header + buf) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 n = write(self._handle, buf) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 BrokenPipeError: [Errno 32] Broken pipe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:28 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:28 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:41:28 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409688.949668068] [moveit_3371273600.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_sphere_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py::test_pick_and_place_sphere_by_position[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. -=================== 1 failed, 1 warning in 176.33s (0:02:56) =================== - - -12:41:35.97 [INFO] Long running tasks: - 537.77s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:42:06.01 [INFO] Long running tasks: - 567.81s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:42:36.04 [INFO] Long running tasks: - 597.84s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:42:38.30 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:44:06.17 [INFO] Long running tasks: - 87.87s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:44:36.21 [INFO] Long running tasks: - 117.91s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:45:06.25 [INFO] Long running tasks: - 147.94s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:45:36.28 [INFO] Long running tasks: - 177.98s Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:46:01.14 [INFO] Completed: Scheduling: Run Pytest for src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests -12:46:01.15 [ERROR] Completed: Run Pytest - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests - failed (exit code 1). -============================= test session starts ============================== -platform linux -- Python 3.12.3, pytest-9.0.2, pluggy-1.6.0 -rootdir: /tmp/pants-sandbox-WzZgMH -configfile: pytest.ini -asyncio: mode=Mode.STRICT, debug=False, asyncio_default_fixture_loop_scope=None, asyncio_default_test_loop_scope=function -collected 1 item - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py::test_pick_and_place_cylinder_by_position[ur5e] FAILED [100%] ------------------------------- live log teardown ------------------------------- -2026-05-10 12:45:11 [ ERROR] [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-42-39-321904-DESKTOP-HG3Q5KV-11687 (testutils.py:33) - -2026-05-10 12:45:11 [ ERROR] [INFO] [launch]: Default logging verbosity is set to INFO (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [ros2_control_node-1]: process started with pid [11697] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [robot_state_publisher-2]: process started with pid [11698] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [trajectory_until_node-3]: process started with pid [11699] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-4]: process started with pid [11700] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-5]: process started with pid [11701] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [robot_state_publisher-2] [INFO] [1778409759.572370442] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.575114270] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.577347037] [controller_manager]: Subscribing to '/robot_description' topic for robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.579718092] [controller_manager]: update rate is 500 Hz (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.579745054] [controller_manager]: Overruns handling is : enabled (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.579750444] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409759.579841808] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.587985256] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.590568079] [controller_manager]: Loading hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.590936359] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.591006081] [controller_manager]: Initialize hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.597676025] [controller_manager]: Successful initialization of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598778450] [resource_manager]: 'configure' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598806593] [resource_manager]: Successful 'configure' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598826561] [resource_manager]: 'activate' hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598830839] [resource_manager]: Successful 'activate' of hardware 'ur5e' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598875915] [controller_manager]: Registering statistics for : ur5e (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409759.598906874] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409759.796350801] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.048086102] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.048137820] [controller_manager]: Loading controller 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.050369014] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.063311131] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.064269262] [controller_manager]: Configuring controller: 'joint_state_broadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.064390081] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.071389411] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.072355622] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.075025881] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.075615661] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.075650738] [controller_manager]: Loading controller 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.079958879] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.090902207] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.091127927] [controller_manager]: Configuring controller: 'io_and_status_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.092898246] [controller_manager]: Activating controllers: [ io_and_status_controller ] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409760.102387266] [controller_manager]: Overrun might occur, Total time : 8218.786 us (Expected < 2000.000 us) --> Read time : 12.393 us, Update time : 8204.279 us (Switch time : 8164.122 us (Switch chained mode time : 0.221 us, perform mode change time : 9.688 us, Activation time : 8137.792 us, Deactivation time : 0.080 us)), Write time : 2.114 us (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409760.102436369] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.361093 ms (missed cycles : 5). (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.102400562] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.104897746] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.105534206] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.105589781] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.105927302] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.113736721] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.114973832] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.115183214] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.115243960] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.116900007] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.118259791] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.121255458] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.121986862] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.122016658] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.122926768] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.136838878] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.137019340] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.138912768] [force_torque_sensor_broadcaster]: configure successful (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.140936653] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.142264922] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.144992661] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.145726586] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.145756282] [controller_manager]: Loading controller 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.146024652] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.157190588] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.157438333] [controller_manager]: Configuring controller: 'ur_configuration_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.160904966] [controller_manager]: Activating controllers: [ ur_configuration_controller ] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.162301847] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.165009810] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.165995709] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.166026167] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.166321508] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.175373999] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.187133253] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187351883] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187471952] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187490887] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.187507800] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.189338703] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.196883635] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.198482022] [controller_manager]: Successfully switched controllers! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-4] [INFO] [1778409760.201254175] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.300896948] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-4]: process has finished cleanly [pid 11700] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.552925686] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.552971624] [controller_manager]: Loading controller 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.553297012] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.577285509] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578372249] [controller_manager]: Configuring controller: 'joint_trajectory_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578501324] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578534357] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.578549736] [joint_trajectory_controller]: Using 'splines' interpolation method. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.579896739] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.585454410] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.585485018] [controller_manager]: Loading controller 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.586866323] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.599101881] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.599337505] [controller_manager]: Configuring controller: 'forward_velocity_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.599933978] [forward_velocity_controller]: configure successful (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.601466960] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.601501486] [controller_manager]: Loading controller 'forward_position_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.602178979] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.615091618] [spawner_joint_trajectory_controller]: Loaded forward_position_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.615361431] [controller_manager]: Configuring controller: 'forward_position_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.615856091] [forward_position_controller]: configure successful (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.617339460] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.617374387] [controller_manager]: Loading controller 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.617963511] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.631015802] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.631256749] [controller_manager]: Configuring controller: 'forward_effort_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.631834047] [forward_effort_controller]: configure successful (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.633593280] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.633629018] [controller_manager]: Loading controller 'force_mode_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.633921754] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.647113599] [spawner_joint_trajectory_controller]: Loaded force_mode_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.647375633] [controller_manager]: Configuring controller: 'force_mode_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.657399630] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.657454774] [controller_manager]: Loading controller 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.657766216] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.671224127] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.671490659] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.677654796] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.677686366] [controller_manager]: Loading controller 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.677956966] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.691288188] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.691542766] [controller_manager]: Configuring controller: 'freedrive_mode_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.693839549] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.693868144] [controller_manager]: Loading controller 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.694081689] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [spawner-5] [INFO] [1778409760.707342912] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409760.707635995] [controller_manager]: Configuring controller: 'tool_contact_controller' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [spawner-5]: process has finished cleanly [pid 11701] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [robot_state_publisher-2] [INFO] [1778409762.154008266] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 11698] (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409762.441361682] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409762.441407409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409764.366208387] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.132930 ms (missed cycles : 4). (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409765.470213376] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.138190 ms (missed cycles : 3). (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409767.370228047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.152900 ms (missed cycles : 2). (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409767.764106413] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409767.764159974] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409768.764221564] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409768.764270436] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409769.764353752] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409769.764403917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409770.764279850] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409770.764331438] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409771.764296829] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409771.764348337] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409772.764151065] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409772.764195329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409773.764308030] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409773.764363706] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409774.764317523] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409774.764376144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409775.764264462] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409775.764315809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409776.764271604] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409776.764333021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409777.764201295] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409777.764256730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409778.764213293] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409778.764258990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409779.764320319] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409779.764402566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409780.764376270] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409780.764428580] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409781.764234109] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409781.764284775] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409782.764221240] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409782.764277897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409783.764200595] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409783.764253175] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409784.764206302] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409784.764260665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409785.764284875] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409785.764335051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409786.764236769] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409786.764274701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409787.764304257] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409787.764419431] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409788.764223979] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409788.764272491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409789.764202853] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409789.764256505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409790.764193047] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409790.764256858] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409791.764231436] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409791.764286090] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409792.560261504] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409792.764167109] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409792.764212796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409793.111256952] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409793.764242579] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409793.764315818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409794.664282440] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409794.664339488] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409794.764221255] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409794.764293623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409795.764231286] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409795.764283425] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409796.219305492] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409796.219378792] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409796.764140746] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409796.764188406] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409797.686221356] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409797.764151306] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409797.764194769] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409798.764168300] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409798.764231409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409799.764138819] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409799.764192160] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409800.764216317] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409800.764271802] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409801.764173245] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409801.764225634] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409802.735369275] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409802.764235980] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409802.764284542] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409803.286375253] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409803.764228797] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409803.764289833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409804.202246564] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409804.202289115] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409804.764350223] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409804.764403744] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409805.764143247] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409805.764191188] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409806.764178860] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409806.764253532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409807.764154195] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409807.764204591] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409808.764180726] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409808.764230901] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409809.764191153] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409809.764250566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409810.764180819] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409810.764249470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409811.764191596] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409811.764238565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409812.764181212] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409812.764227530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409813.764191675] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409813.764249625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409814.764181026] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409814.764229659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409815.764183111] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409815.764235070] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409816.764308311] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409816.764362654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409816.782527509] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409817.333485734] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409817.764216117] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409817.764269418] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409818.764184825] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409818.764234409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409819.267339971] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409819.267390677] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409819.764203527] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409819.764260024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.606993302] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.607043196] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.748194354] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409820.764149736] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409820.764189692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409821.764104533] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409821.764161561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409822.764151759] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409822.764223735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409823.764083220] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409823.764136411] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409824.764143506] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409824.764198079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409824.766423494] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409825.317389150] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409825.764192539] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409825.764233758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409826.764187190] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409826.764257223] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409827.764097767] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409827.764145818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409828.024658497] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409828.024714844] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409828.764153389] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409828.764204746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409829.764194344] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409829.764249919] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409830.764183563] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409830.764231384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409831.764209764] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409831.764259027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409832.764289690] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409832.764339715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409833.764213431] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409833.764263245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409834.764203131] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409834.764252324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409835.764172697] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409835.764224516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409836.764150921] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409836.764198401] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409837.764182699] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409837.764239306] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409838.340023718] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409838.764146377] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409838.764190791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409838.891018373] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409839.764202651] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409839.764240242] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409840.083369699] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.083423341] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.764216716] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409840.764281318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.871145575] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409840.871208274] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409841.764198548] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409841.764251038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409842.668214952] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409842.764281857] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409842.764335960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409843.764142655] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409843.764192810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409844.764157054] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409844.764230965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409845.764392813] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409845.764455562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409846.764194145] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409846.764248037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409847.764208528] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409847.764249636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409848.764175566] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409848.764228657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409849.533039064] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409849.764145599] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409849.764197628] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409850.083961123] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409850.764292220] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409850.764342245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409850.864032152] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409850.864078340] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409851.764195297] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409851.764274027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409852.764175676] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409852.764232103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409853.764172091] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409853.764220353] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409854.764177934] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409854.764228691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409855.764216244] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409855.764271629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409856.764091563] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409856.764138863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409857.764163193] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409857.764211815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409858.764203304] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409858.764254541] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409859.764161922] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409859.764208641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409860.764203237] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409860.764255276] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409861.069795227] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409861.620866347] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409861.764215221] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409861.764266990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409862.438528062] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.438587956] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.764335855] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409862.764378456] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.767017921] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.767074939] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409862.962206344] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409863.764169838] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409863.764219502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409864.764191833] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409864.764242560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409865.764171424] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409865.764229274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409866.764185664] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409866.764240809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409867.764406392] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409867.764454714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409867.960177944] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409868.511163799] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409868.764178218] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409868.764236378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409869.255438544] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409869.255499320] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409869.764144008] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409869.764197760] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409870.764189251] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409870.764241730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409871.764179272] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409871.764257400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409872.764179042] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409872.764254194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409873.764170001] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409873.764238240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409874.764203882] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409874.764273675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409875.764251384] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409875.764307981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409876.764219542] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409876.764274557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409877.764198201] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409877.764267423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409878.764162437] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409878.764212001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409879.764204727] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409879.764260693] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409879.948926906] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409880.499862929] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409880.764196880] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409880.764254880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409881.764169136] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409881.764239410] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409881.983318541] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409882.534276948] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409882.764203731] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409882.764260399] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409883.387738055] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.387793610] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.551063022] [scaled_joint_trajectory_controller]: Received new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.551120781] [scaled_joint_trajectory_controller]: Accepted new action goal (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.760222550] [scaled_joint_trajectory_controller]: Goal reached, success! (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409883.764180751] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409883.764220707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409884.764142597] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409884.764195047] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409885.764155987] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409885.764202947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409886.764160882] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409886.764204946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409887.764237674] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409887.764295594] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409888.764190966] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409888.764249117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409889.363671580] [io_and_status_controller]: Setting speed slider to 50.00%. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409889.764147673] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409889.764195784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409889.914686842] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409890.764151902] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409890.764221915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409890.828126416] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409890.828182784] [io_and_status_controller]: Payload has been set successfully (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409891.764203500] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409891.764251812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409892.764138920] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409892.764187272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409893.764178169] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409893.764250006] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409894.764175368] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409894.764223350] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [INFO] [1778409895.764221848] [controller_manager]: Received robot description from topic. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [ros2_control_node-1] [WARN] [1778409895.764274678] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [1778409762.319032737] [robot_state_publisher]: Robot initialized (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] [INFO] [1778409911.373843058] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] * Serving Flask app 'arcor2_storage.scripts.storage' (testutils.py:33) -2026-05-10 12:45:11 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] WARNING:root:The robot description will be loaded from /robot_description topic  (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:47 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] * Serving Flask app 'arcor2_ur.scripts.ur' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] * Debug mode: off (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] * Running on all addresses (0.0.0.0) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] * Running on http://127.0.0.1:58727 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] * Running on http://172.30.43.138:58727 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] Press CTRL+C to quit (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "GET /healthz/ready HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "PUT /system/start HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "GET /system/running HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:42:47] "GET /state/started HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.257718930] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize rclcpp (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.257775468] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node parameters (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.257788693] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node and executor (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.264980653] [moveit_1502777475.moveit.py.cpp_initializer]: Spin separate thread (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.266709778] [moveit_1502777475.moveit.ros.rdf_loader]: Loaded robot model in 0.00161847 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.266735237] [moveit_1502777475.moveit.core.robot_model]: Loading robot model 'ur5e'... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.266743102] [moveit_1502777475.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409772.279139666] [moveit_1502777475.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.281573511] [moveit_1502777475.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.334319664] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.334435014] [moveit_1502777475.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.335323071] [moveit_1502777475.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.335873668] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.335886742] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.336014686] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.336392794] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.336480110] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting planning scene monitor (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337085761] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337097563] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337542359] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.337993165] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409772.338242710] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409772.338258289] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.820442676] [moveit_1502777475.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.821083354] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822069167] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822079546] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822369427] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822385167] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822410565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822416186] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.822426466] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.823013311] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.825261203] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.825276572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.827036275] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.827049400] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.827853978] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.856359461] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.859791132] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.859954893] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.859977827] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409772.860833573] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:52 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:54 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:55 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:55 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:57 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:42:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:43:09] "PUT /state/start HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:43:09] "GET /joints HTTP/1.1" 200 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 127.0.0.1 - - [10/May/2026 12:43:10] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:10 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:10 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:11 INFO  Generated 20 grasp options for object Cyl1. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  IK accepted: 20 grasp options. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:12 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:14 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:14 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:15 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:15 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153443391] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153563640] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153577316] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.153595170] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409796.153662067] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.154085602] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.154206211] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.169621863] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.170072870] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.170416763] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Executing plan (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216943760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216965231] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216983004] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.216989677] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.217007110] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218558112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218597668] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218622855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.218775075] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.219582153] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409796.219600488] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:16 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409797.720152829] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409797.724501172] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:17 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:18 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:18 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:19 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:19 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:20 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:20 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:25 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430164123] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430238374] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430252491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430265947] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409805.430344977] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430586536] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409805.430635269] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.433369314] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.433788250] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.433897812] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.434094120] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409815.434251880] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003569 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.442111916] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409815.442221494] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:35 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:35 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:36 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:36 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:38 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:38 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584147221] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584203178] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584211964] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584223005] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409820.584299541] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584518948] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.584598829] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.597357623] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.597428688] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.597522907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:40 INFO  Executing plan (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604442585] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604467592] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604485016] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604493532] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.604512668] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606562112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606591648] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606612728] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.606725563] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.607298807] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.607312493] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.757777293] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409820.762481005] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:41 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:41 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:43 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:47 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:48 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309517858] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309572281] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309580797] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309592019] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409828.309701827] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.309951271] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409828.310044769] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.313605646] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.313763095] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.314096699] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.314217483] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409838.314339981] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004222 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.335261686] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409838.335320918] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:58 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:59 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:43:59 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.334937564] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.334986878] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.334994031] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.335004962] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409840.335072430] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.335303730] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.335344007] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.361554961] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.362156544] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.362570550] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:00 INFO  Executing plan (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868328733] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868368077] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868408855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868422000] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.868454421] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870634349] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870682330] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870704953] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.870838467] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.871434003] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409840.871448691] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409842.671872612] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409842.676467122] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:07 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:07 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:08 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:08 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:09 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:09 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:11 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:11 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:11 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005531569] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005608716] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005620558] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005633563] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409851.005712773] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.005958300] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409851.006008856] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008170904] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008356352] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008610689] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.008726895] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409861.008897064] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.002831 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.027596805] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409861.027655586] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:21 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724713760] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724773343] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724782410] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.724793952] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409862.724870548] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.725097599] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.725148887] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.741512678] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.741581709] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.741639619] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Executing plan (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765078426] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765101149] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765115166] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765122760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.765139312] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766605713] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766639918] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766661479] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.766773902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.767293940] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.767308498] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:22 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.967715412] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409862.972475057] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:24 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:25 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:25 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:26 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:26 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:27 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:27 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:28 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:28 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:29 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:29 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:29 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903117283] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903168130] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903175123] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903184341] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409869.903280754] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903521070] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409869.903615410] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.906350239] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.906886684] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.907037235] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.907146348] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409879.907300711] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003617 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.930359365] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409879.930435400] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:39 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:39 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:42 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:43 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544616182] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544667620] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544674794] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544684552] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409883.544758092] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.544983991] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.545065927] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.548860186] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.548946149] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549026222] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:43 INFO  Executing plan (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549221883] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549240869] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549254145] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549261188] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.549276377] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550556860] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550584162] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550612315] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.550723767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.551306820] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.551322309] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.801675456] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409883.806462571] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:44 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:44 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:45 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:45 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:46 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:48 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:48 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:49 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:49 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:50 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:50 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:51 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:51 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:44:51 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023659043] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023709839] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023716782] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.023726441] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409891.023790152] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.024005240] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409891.024046759] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.026011506] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.027183142] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.027674426] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.027763870] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409901.027877612] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003787 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.030747966] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409901.030807449] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:01 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478695882] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478746709] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478753822] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.478763250] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409902.478825398] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.479049083] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.479089971] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.491502647] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.491578932] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.491652091] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Executing plan (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658462590] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658489952] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658507485] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658516202] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.658536400] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660578857] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660618292] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660643460] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.660760553] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.661270336] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.661281607] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:02 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.861545943] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409902.866470690] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:03 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:03 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:04 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:04 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:05 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:05 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:06 INFO  Robot mode: 7 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:06 INFO  Program running: True (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:07 INFO  Planning trajectory (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578555476] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578604820] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578613286] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578622273] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409907.578684071] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578887607] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409907.578929567] [moveit_py]: Calling Planner 'OMPL' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409911.502946530] [rclcpp]: signal_handler(signum=15) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.582781701] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.582860536] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.583501359] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.583585333] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [WARN] [1778409917.583696751] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004731 seconds (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.604220980] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [ERROR] [1778409917.604280282] [moveit_py]: Planner 'OMPL' failed with error code FAILURE (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:43 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] result = runtime.attach_object( (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] raise UrGeneral(f"Unable to attach object {object_id}.") (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:43 ERROR  ROS worker command failed. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] conn.send({"status": "ok", "result": result}) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self._send_bytes(_ForkingPickler.dumps(obj)) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self._send(header + buf) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] n = write(self._handle, buf) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] BrokenPipeError: [Errno 32] Broken pipe (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:58 ERROR  Failed to switch controllers off during shutdown. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self._switch_freedrive_controller(False) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] active = self._active_controllers() (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] raise UrGeneral("Service call timed out.") (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] arcor2_ur.exceptions.UrGeneral: Service call timed out. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] >>> [rcutils|error_handling.c:108] rcutils_set_error_state() (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] This error state is being overwritten: (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] with this new error message: (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 'publisher's context is invalid, at ./src/rcl/publisher.c:423' (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] rcutils_reset_error() should be called after error handling to avoid this. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] <<< (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:58 ERROR  Failed to clear freedrive mode during shutdown. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self.node.set_freedrive_mode(False) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self._publish_freedrive_mode(enabled) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self.freedrive_mode_pub.publish(msg) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self.__publisher.publish(msg) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] 2026-05-10 12:45:58 ERROR  Failed to shutdown rclpy. (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] Traceback (most recent call last): (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] rclpy.shutdown() (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] _shutdown(context=context) (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] context.shutdown() (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] self.__context.shutdown() (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 (testutils.py:33) -2026-05-10 12:46:00 [ ERROR] [INFO] [1778409958.792785146] [moveit_1502777475.moveit.ros.moveit_cpp]: Deleting MoveItCpp (testutils.py:33) - - -=================================== FAILURES =================================== -________________ test_pick_and_place_cylinder_by_position[ur5e] ________________ - -self = -conn = -method = 'PUT', url = '/graspable/pick_up_object_by_position' -body = b'{"position":{"x":0.0,"y":0.5,"z":0.1},"radius":0.5,"effectorType":"SUCK","graspPositions":["TOP"],"velocity":50.0,"payload":0.0,"safe":true}' -headers = {'User-Agent': 'python-requests/2.32.5', 'Accept-Encoding': 'gzip, deflate', 'accept': 'application/json', 'Connection': 'keep-alive', 'content-type': 'application/json; charset=utf-8', 'Content-Length': '140'} -retries = Retry(total=0, connect=None, read=False, redirect=None, status=None) -timeout = Timeout(connect=3.05, read=120, total=None), chunked = False -response_conn = -preload_content = False, decode_content = False, enforce_content_length = True - - def _make_request( - self, - conn: BaseHTTPConnection, - method: str, - url: str, - body: _TYPE_BODY | None = None, - headers: typing.Mapping[str, str] | None = None, - retries: Retry | None = None, - timeout: _TYPE_TIMEOUT = _DEFAULT_TIMEOUT, - chunked: bool = False, - response_conn: BaseHTTPConnection | None = None, - preload_content: bool = True, - decode_content: bool = True, - enforce_content_length: bool = True, - ) -> BaseHTTPResponse: -  """ -  Perform a request on a given urllib connection object taken from our -  pool. -  -  :param conn: -  a connection from one of our connection pools -  -  :param method: -  HTTP request method (such as GET, POST, PUT, etc.) -  -  :param url: -  The URL to perform the request on. -  -  :param body: -  Data to send in the request body, either :class:`str`, :class:`bytes`, -  an iterable of :class:`str`/:class:`bytes`, or a file-like object. -  -  :param headers: -  Dictionary of custom headers to send, such as User-Agent, -  If-None-Match, etc. If None, pool headers are used. If provided, -  these headers completely replace any pool-specific headers. -  -  :param retries: -  Configure the number of retries to allow before raising a -  :class:`~urllib3.exceptions.MaxRetryError` exception. -  -  Pass ``None`` to retry until you receive a response. Pass a -  :class:`~urllib3.util.retry.Retry` object for fine-grained control -  over different types of retries. -  Pass an integer number to retry connection errors that many times, -  but no other types of errors. Pass zero to never retry. -  -  If ``False``, then retries are disabled and any exception is raised -  immediately. Also, instead of raising a MaxRetryError on redirects, -  the redirect response will be returned. -  -  :type retries: :class:`~urllib3.util.retry.Retry`, False, or an int. -  -  :param timeout: -  If specified, overrides the default timeout for this one -  request. It may be a float (in seconds) or an instance of -  :class:`urllib3.util.Timeout`. -  -  :param chunked: -  If True, urllib3 will send the body using chunked transfer -  encoding. Otherwise, urllib3 will send the body using the standard -  content-length form. Defaults to False. -  -  :param response_conn: -  Set this to ``None`` if you will handle releasing the connection or -  set the connection to have the response release it. -  -  :param preload_content: -  If True, the response's body will be preloaded during construction. -  -  :param decode_content: -  If True, will attempt to decode the body based on the -  'content-encoding' header. -  -  :param enforce_content_length: -  Enforce content length checking. Body returned by server must match -  value of Content-Length header, if present. Otherwise, raise error. -  """ - self.num_requests += 1 -  - timeout_obj = self._get_timeout(timeout) - timeout_obj.start_connect() - conn.timeout = Timeout.resolve_default_timeout(timeout_obj.connect_timeout) -  - try: - # Trigger any extra validation we need to do. - try: - self._validate_conn(conn) - except (SocketTimeout, BaseSSLError) as e: - self._raise_timeout(err=e, url=url, timeout_value=conn.timeout) - raise -  - # _validate_conn() starts the connection to an HTTPS proxy - # so we need to wrap errors with 'ProxyError' here too. - except ( - OSError, - NewConnectionError, - TimeoutError, - BaseSSLError, - CertificateError, - SSLError, - ) as e: - new_e: Exception = e - if isinstance(e, (BaseSSLError, CertificateError)): - new_e = SSLError(e) - # If the connection didn't successfully connect to it's proxy - # then there - if isinstance( - new_e, (OSError, NewConnectionError, TimeoutError, SSLError) - ) and (conn and conn.proxy and not conn.has_connected_to_proxy): - new_e = _wrap_proxy_error(new_e, conn.proxy.scheme) - raise new_e -  - # conn.request() calls http.client.*.request, not the method in - # urllib3.request. It also calls makefile (recv) on the socket. - try: - conn.request( - method, - url, - body=body, - headers=headers, - chunked=chunked, - preload_content=preload_content, - decode_content=decode_content, - enforce_content_length=enforce_content_length, - ) -  - # We are swallowing BrokenPipeError (errno.EPIPE) since the server is - # legitimately able to close the connection after sending a valid response. - # With this behaviour, the received response is still readable. - except BrokenPipeError: - pass - except OSError as e: - # MacOS/Linux - # EPROTOTYPE and ECONNRESET are needed on macOS - # https://erickt.github.io/blog/2014/11/19/adventures-in-debugging-a-potential-osx-kernel-bug/ - # Condition changed later to emit ECONNRESET instead of only EPROTOTYPE. - if e.errno != errno.EPROTOTYPE and e.errno != errno.ECONNRESET: - raise -  - # Reset the timeout for the recv() on the socket - read_timeout = timeout_obj.read_timeout -  - if not conn.is_closed: - # In Python 3 socket.py will catch EAGAIN and return None when you - # try and read into the file pointer created by http.client, which - # instead raises a BadStatusLine exception. Instead of catching - # the exception and assuming all BadStatusLine exceptions are read - # timeouts, check for a zero timeout before making the request. - if read_timeout == 0: - raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={read_timeout})" - ) - conn.timeout = read_timeout -  - # Receive the response from the server - try: -> response = conn.getresponse() - ^^^^^^^^^^^^^^^^^^ - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:534: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connection.py:571: in getresponse - httplib_response = super().getresponse() - ^^^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:1428: in getresponse - response.begin() -/usr/lib/python3.12/http/client.py:331: in begin - version, status, reason = self._read_status() - ^^^^^^^^^^^^^^^^^^^ -/usr/lib/python3.12/http/client.py:292: in _read_status - line = str(self.fp.readline(_MAXLINE + 1), "iso-8859-1") - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -b = - - def readinto(self, b): -  """Read up to len(b) bytes into the writable buffer *b* and return -  the number of bytes read. If the socket is non-blocking and no bytes -  are available, None is returned. -  -  If *b* is non-empty, a 0 return value indicates that the connection -  was shutdown at the other end. -  """ - self._checkClosed() - self._checkReadable() - if self._timeout_occurred: - raise OSError("cannot read from timed out object") - while True: - try: -> return self._sock.recv_into(b) - ^^^^^^^^^^^^^^^^^^^^^^^ -E TimeoutError: timed out - -/usr/lib/python3.12/socket.py:707: TimeoutError - -The above exception was the direct cause of the following exception: - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: -> resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:644: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:841: in urlopen - retries = retries.increment( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/retry.py:490: in increment - raise reraise(type(error), error, _stacktrace) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/util/util.py:39: in reraise - raise value -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:787: in urlopen - response = self._make_request( -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:536: in _make_request - self._raise_timeout(err=e, url=url, timeout_value=read_timeout) -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -err = TimeoutError('timed out'), url = '/graspable/pick_up_object_by_position' -timeout_value = 120 - - def _raise_timeout( - self, - err: BaseSSLError | OSError | SocketTimeout, - url: str, - timeout_value: _TYPE_TIMEOUT | None, - ) -> None: -  """Is the error actually a timeout? Will raise a ReadTimeout or pass""" -  - if isinstance(err, SocketTimeout): -> raise ReadTimeoutError( - self, url, f"Read timed out. (read timeout={timeout_value})" - ) from err -E urllib3.exceptions.ReadTimeoutError: HTTPConnectionPool(host='0.0.0.0', port=58727): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/urllib3/connectionpool.py:367: ReadTimeoutError - -During handling of the above exception, another exception occurred: - -method = )> -url = 'http://0.0.0.0:58727/graspable/pick_up_object_by_position' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: -> resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - -src/python/arcor2_web/rest.py:259: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:130: in put - return request("put", url, data=data, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/api.py:59: in request - return session.request(method=method, url=url, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:589: in request - resp = self.send(prep, **send_kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/sessions.py:703: in send - r = adapter.send(request, **kwargs) - ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -self = -request = , stream = False -timeout = Timeout(connect=3.05, read=120, total=None), verify = True -cert = None, proxies = OrderedDict() - - def send( - self, request, stream=False, timeout=None, verify=True, cert=None, proxies=None - ): -  """Sends PreparedRequest object. Returns Response object. -  -  :param request: The :class:`PreparedRequest ` being sent. -  :param stream: (optional) Whether to stream the request content. -  :param timeout: (optional) How long to wait for the server to send -  data before giving up, as a float, or a :ref:`(connect timeout, -  read timeout) ` tuple. -  :type timeout: float or tuple or urllib3 Timeout object -  :param verify: (optional) Either a boolean, in which case it controls whether -  we verify the server's TLS certificate, or a string, in which case it -  must be a path to a CA bundle to use -  :param cert: (optional) Any user-provided SSL certificate to be trusted. -  :param proxies: (optional) The proxies dictionary to apply to the request. -  :rtype: requests.Response -  """ -  - try: - conn = self.get_connection_with_tls_context( - request, verify, proxies=proxies, cert=cert - ) - except LocationValueError as e: - raise InvalidURL(e, request=request) -  - self.cert_verify(conn, request.url, verify, cert) - url = self.request_url(request, proxies) - self.add_headers( - request, - stream=stream, - timeout=timeout, - verify=verify, - cert=cert, - proxies=proxies, - ) -  - chunked = not (request.body is None or "Content-Length" in request.headers) -  - if isinstance(timeout, tuple): - try: - connect, read = timeout - timeout = TimeoutSauce(connect=connect, read=read) - except ValueError: - raise ValueError( - f"Invalid timeout {timeout}. Pass a (connect, read) timeout tuple, " - f"or a single float to set both timeouts to the same value." - ) - elif isinstance(timeout, TimeoutSauce): - pass - else: - timeout = TimeoutSauce(connect=timeout, read=timeout) -  - try: - resp = conn.urlopen( - method=request.method, - url=url, - body=request.body, - headers=request.headers, - redirect=False, - assert_same_host=False, - preload_content=False, - decode_content=False, - retries=self.max_retries, - timeout=timeout, - chunked=chunked, - ) -  - except (ProtocolError, OSError) as err: - raise ConnectionError(err, request=request) -  - except MaxRetryError as e: - if isinstance(e.reason, ConnectTimeoutError): - # TODO: Remove this in 3.0.0: see #2811 - if not isinstance(e.reason, NewConnectionError): - raise ConnectTimeout(e, request=request) -  - if isinstance(e.reason, ResponseError): - raise RetryError(e, request=request) -  - if isinstance(e.reason, _ProxyError): - raise ProxyError(e, request=request) -  - if isinstance(e.reason, _SSLError): - # This branch is for urllib3 v1.22 and later. - raise SSLError(e, request=request) -  - raise ConnectionError(e, request=request) -  - except ClosedPoolError as e: - raise ConnectionError(e, request=request) -  - except _ProxyError as e: - raise ProxyError(e) -  - except (_SSLError, _HTTPError) as e: - if isinstance(e, _SSLError): - # This branch is for urllib3 versions earlier than v1.22 - raise SSLError(e, request=request) - elif isinstance(e, ReadTimeoutError): -> raise ReadTimeout(e, request=request) -E requests.exceptions.ReadTimeout: HTTPConnectionPool(host='0.0.0.0', port=58727): Read timed out. (read timeout=120) - -/home/kado/.cache/pants/named_caches/pex_root/venvs/3/s/7df43922/venv/lib/python3.12/site-packages/requests/adapters.py:690: ReadTimeout - -The above exception was the direct cause of the following exception: - -start_processes = Urls(ros_domain_id='220', robot_url='http://0.0.0.0:58727', storage_url='http://127.0.0.1:41809') - - @pytest.mark.timeout(321) - def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None: - scene_service.URL = start_processes.robot_url - scene_service.start() - assert scene_service.started() -  - X = 0.0 - Y = 0.5 - Z = 0.1 -  - ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) - assert len(ot.robot_joints()) == 6 -  - object = Cylinder("Cyl1", 0.1, 0.2) - scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) - time.sleep(1) -  -> ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:30: -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -src/python/arcor2_ur/object_types/ur5e.py:195: in pick_up_object_by_position - rest.call( -_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - -method = )> -url = 'http://0.0.0.0:58727/graspable/pick_up_object_by_position' - - def call( - method: Method, - url: str, - *, - return_type: ReturnType = None, - list_return_type: ReturnType = None, - body: OptBody = None, - params: OptParams = None, - raw_params: bool = False, - files: OptFiles = None, - timeout: OptTimeout = None, - ) -> ReturnValue: -  """Universal function for calling REST APIs. -  -  :param method: HTTP method. -  :param url: Resource address. -  :param return_type: If set, function will try to return one value of a given type. -  :param list_return_type: If set, function will try to return list of a given type. -  :param body: Data to be send in the request body. -  :param params: Path parameters. -  :param raw_params: Parameters are by default converted to camelCase form. -  If you do not want this behavior, pass True here -  :param files: Instead of body, it is possible to send files. -  :param timeout: Specific timeout for a call. -  :return: Return value/type is given by return_type/list_return_type. If both are None, nothing will be returned. -  """ - logger.debug(f"{method} {url}, body: {body}, params: {params}, files: {files is not None}, timeout: {timeout}") -  - if body and files: - raise RestException("Can't send data and files at the same time.") -  - if return_type and list_return_type: - raise RestException("Only one argument from 'return_type' and 'list_return_type' can be used.") -  - if return_type is None: - return_type = list_return_type -  - d: list[Any] | dict[str, Any] | None = None -  - # prepare body data into dict or list (if any) - if isinstance(body, JsonSchemaMixin): - d = humps.camelize(body.to_dict()) - elif isinstance(body, list): - d = [] - for dd in body: - if isinstance(dd, JsonSchemaMixin): - d.append(humps.camelize(dd.to_dict())) - else: - d.append(dd) - elif body is not None: - raise RestException("Unsupported type of data.") -  - if params and not raw_params: - params = humps.camelize(params) -  - # requests just simply stringifies parameters, which does not work for booleans - for param_name, param_value in params.items(): - if isinstance(param_value, bool): - params[param_name] = "true" if param_value else "false" -  - if timeout is None: - timeout = Timeout() -  - try: - if files: - files = humps.camelize(files) - resp = method.value(url, files=files, timeout=timeout, params=params) - else: - resp = method.value( - url, - data=json.dumps(d).encode("utf-8") if d is not None else None, - timeout=timeout, - headers=headers, - params=params, - ) - except requests.exceptions.RequestException as e: - logger.debug("Request failed.", exc_info=True) - # TODO would be good to provide more meaningful message but the original one could be very very long -> raise RestException("Catastrophic system error.") from e -E arcor2_web.rest.RestException: Catastrophic system error. - -src/python/arcor2_web/rest.py:269: RestException ----------------------------- Captured log teardown ----------------------------- -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: All log files can be found below /tmp/.ros/log/2026-05-10-12-42-39-321904-DESKTOP-HG3Q5KV-11687 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [launch]: Default logging verbosity is set to INFO -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [ros2_control_node-1]: process started with pid [11697] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process started with pid [11698] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [trajectory_until_node-3]: process started with pid [11699] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process started with pid [11700] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process started with pid [11701] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409759.572370442] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.575114270] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.577347037] [controller_manager]: Subscribing to '/robot_description' topic for robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.579718092] [controller_manager]: update rate is 500 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.579745054] [controller_manager]: Overruns handling is : enabled -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.579750444] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409759.579841808] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.587985256] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.590568079] [controller_manager]: Loading hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.590936359] [controller_manager]: Loaded hardware 'ur5e' from plugin 'mock_components/GenericSystem' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.591006081] [controller_manager]: Initialize hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.597676025] [controller_manager]: Successful initialization of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598778450] [resource_manager]: 'configure' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598806593] [resource_manager]: Successful 'configure' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598826561] [resource_manager]: 'activate' hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598830839] [resource_manager]: Successful 'activate' of hardware 'ur5e' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598875915] [controller_manager]: Registering statistics for : ur5e -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409759.598906874] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409759.796350801] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.048086102] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.048137820] [controller_manager]: Loading controller 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.050369014] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.063311131] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.064269262] [controller_manager]: Configuring controller: 'joint_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.064390081] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.071389411] [controller_manager]: Activating controllers: [ joint_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.072355622] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.075025881] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.075615661] [controller_manager]: Loading controller : 'io_and_status_controller' of type 'ur_controllers/GPIOController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.075650738] [controller_manager]: Loading controller 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.079958879] [controller_manager]: Controller 'io_and_status_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.090902207] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.091127927] [controller_manager]: Configuring controller: 'io_and_status_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.092898246] [controller_manager]: Activating controllers: [ io_and_status_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409760.102387266] [controller_manager]: Overrun might occur, Total time : 8218.786 us (Expected < 2000.000 us) --> Read time : 12.393 us, Update time : 8204.279 us (Switch time : 8164.122 us (Switch chained mode time : 0.221 us, perform mode change time : 9.688 us, Activation time : 8137.792 us, Deactivation time : 0.080 us)), Write time : 2.114 us -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409760.102436369] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 8.361093 ms (missed cycles : 5). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.102400562] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.104897746] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.105534206] [controller_manager]: Loading controller : 'speed_scaling_state_broadcaster' of type 'ur_controllers/SpeedScalingStateBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.105589781] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.105927302] [controller_manager]: Controller 'speed_scaling_state_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.113736721] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.114973832] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.115183214] [controller_manager]: Configuring controller: 'speed_scaling_state_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.115243960] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.116900007] [controller_manager]: Activating controllers: [ speed_scaling_state_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.118259791] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.121255458] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.121986862] [controller_manager]: Loading controller : 'force_torque_sensor_broadcaster' of type 'force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.122016658] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.122926768] [controller_manager]: Controller 'force_torque_sensor_broadcaster' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.136838878] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.137019340] [controller_manager]: Configuring controller: 'force_torque_sensor_broadcaster' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.138912768] [force_torque_sensor_broadcaster]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.140936653] [controller_manager]: Activating controllers: [ force_torque_sensor_broadcaster ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.142264922] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.144992661] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.145726586] [controller_manager]: Loading controller : 'ur_configuration_controller' of type 'ur_controllers/URConfigurationController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.145756282] [controller_manager]: Loading controller 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.146024652] [controller_manager]: Controller 'ur_configuration_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.157190588] [spawner_joint_state_broadcaster]: Loaded ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.157438333] [controller_manager]: Configuring controller: 'ur_configuration_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.160904966] [controller_manager]: Activating controllers: [ ur_configuration_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.162301847] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.165009810] [spawner_joint_state_broadcaster]: Configured and activated ur_configuration_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.165995709] [controller_manager]: Loading controller : 'scaled_joint_trajectory_controller' of type 'ur_controllers/ScaledJointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.166026167] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.166321508] [controller_manager]: Controller 'scaled_joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.175373999] [scaled_joint_trajectory_controller]: Using scaling state from the hardware from interface speed_scaling/speed_scaling_factor. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.187133253] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187351883] [controller_manager]: Configuring controller: 'scaled_joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187471952] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187490887] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.187507800] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.189338703] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.196883635] [controller_manager]: Activating controllers: [ scaled_joint_trajectory_controller ] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.198482022] [controller_manager]: Successfully switched controllers! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-4] [INFO] [1778409760.201254175] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.300896948] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-4]: process has finished cleanly [pid 11700] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.552925686] [controller_manager]: Loading controller : 'joint_trajectory_controller' of type 'joint_trajectory_controller/JointTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.552971624] [controller_manager]: Loading controller 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.553297012] [controller_manager]: Controller 'joint_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.577285509] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578372249] [controller_manager]: Configuring controller: 'joint_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578501324] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578534357] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.578549736] [joint_trajectory_controller]: Using 'splines' interpolation method. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.579896739] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.585454410] [controller_manager]: Loading controller : 'forward_velocity_controller' of type 'velocity_controllers/JointGroupVelocityController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.585485018] [controller_manager]: Loading controller 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.586866323] [controller_manager]: Controller 'forward_velocity_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.599101881] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.599337505] [controller_manager]: Configuring controller: 'forward_velocity_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.599933978] [forward_velocity_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.601466960] [controller_manager]: Loading controller : 'forward_position_controller' of type 'position_controllers/JointGroupPositionController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.601501486] [controller_manager]: Loading controller 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.602178979] [controller_manager]: Controller 'forward_position_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.615091618] [spawner_joint_trajectory_controller]: Loaded forward_position_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.615361431] [controller_manager]: Configuring controller: 'forward_position_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.615856091] [forward_position_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.617339460] [controller_manager]: Loading controller : 'forward_effort_controller' of type 'effort_controllers/JointGroupEffortController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.617374387] [controller_manager]: Loading controller 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.617963511] [controller_manager]: Controller 'forward_effort_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.631015802] [spawner_joint_trajectory_controller]: Loaded forward_effort_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.631256749] [controller_manager]: Configuring controller: 'forward_effort_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.631834047] [forward_effort_controller]: configure successful -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.633593280] [controller_manager]: Loading controller : 'force_mode_controller' of type 'ur_controllers/ForceModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.633629018] [controller_manager]: Loading controller 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.633921754] [controller_manager]: Controller 'force_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.647113599] [spawner_joint_trajectory_controller]: Loaded force_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.647375633] [controller_manager]: Configuring controller: 'force_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.657399630] [controller_manager]: Loading controller : 'passthrough_trajectory_controller' of type 'ur_controllers/PassthroughTrajectoryController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.657454774] [controller_manager]: Loading controller 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.657766216] [controller_manager]: Controller 'passthrough_trajectory_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.671224127] [spawner_joint_trajectory_controller]: Loaded passthrough_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.671490659] [controller_manager]: Configuring controller: 'passthrough_trajectory_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.677654796] [controller_manager]: Loading controller : 'freedrive_mode_controller' of type 'ur_controllers/FreedriveModeController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.677686366] [controller_manager]: Loading controller 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.677956966] [controller_manager]: Controller 'freedrive_mode_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.691288188] [spawner_joint_trajectory_controller]: Loaded freedrive_mode_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.691542766] [controller_manager]: Configuring controller: 'freedrive_mode_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.693839549] [controller_manager]: Loading controller : 'tool_contact_controller' of type 'ur_controllers/ToolContactController' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.693868144] [controller_manager]: Loading controller 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.694081689] [controller_manager]: Controller 'tool_contact_controller' node arguments: --ros-args --params-file /opt/ros/jazzy/share/ur_robot_driver/config/ur5e_update_rate.yaml --params-file /tmp/launch_params_xnrhva4o -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [spawner-5] [INFO] [1778409760.707342912] [spawner_joint_trajectory_controller]: Loaded tool_contact_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409760.707635995] [controller_manager]: Configuring controller: 'tool_contact_controller' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [spawner-5]: process has finished cleanly [pid 11701] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [robot_state_publisher-2] [INFO] [1778409762.154008266] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [robot_state_publisher-2]: process has finished cleanly [pid 11698] -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409762.441361682] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409762.441407409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409764.366208387] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 6.132930 ms (missed cycles : 4). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409765.470213376] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 4.138190 ms (missed cycles : 3). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409767.370228047] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 500 Hz. The loop took 2.152900 ms (missed cycles : 2). -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409767.764106413] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409767.764159974] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409768.764221564] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409768.764270436] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409769.764353752] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409769.764403917] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409770.764279850] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409770.764331438] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409771.764296829] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409771.764348337] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409772.764151065] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409772.764195329] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409773.764308030] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409773.764363706] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409774.764317523] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409774.764376144] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409775.764264462] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409775.764315809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409776.764271604] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409776.764333021] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409777.764201295] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409777.764256730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409778.764213293] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409778.764258990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409779.764320319] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409779.764402566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409780.764376270] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409780.764428580] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409781.764234109] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409781.764284775] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409782.764221240] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409782.764277897] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409783.764200595] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409783.764253175] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409784.764206302] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409784.764260665] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409785.764284875] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409785.764335051] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409786.764236769] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409786.764274701] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409787.764304257] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409787.764419431] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409788.764223979] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409788.764272491] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409789.764202853] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409789.764256505] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409790.764193047] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409790.764256858] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409791.764231436] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409791.764286090] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409792.560261504] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409792.764167109] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409792.764212796] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409793.111256952] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409793.764242579] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409793.764315818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409794.664282440] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409794.664339488] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409794.764221255] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409794.764293623] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409795.764231286] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409795.764283425] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409796.219305492] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409796.219378792] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409796.764140746] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409796.764188406] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409797.686221356] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409797.764151306] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409797.764194769] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409798.764168300] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409798.764231409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409799.764138819] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409799.764192160] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409800.764216317] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409800.764271802] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409801.764173245] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409801.764225634] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409802.735369275] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409802.764235980] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409802.764284542] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409803.286375253] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409803.764228797] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409803.764289833] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409804.202246564] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409804.202289115] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409804.764350223] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409804.764403744] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409805.764143247] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409805.764191188] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409806.764178860] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409806.764253532] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409807.764154195] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409807.764204591] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409808.764180726] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409808.764230901] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409809.764191153] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409809.764250566] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409810.764180819] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409810.764249470] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409811.764191596] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409811.764238565] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409812.764181212] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409812.764227530] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409813.764191675] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409813.764249625] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409814.764181026] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409814.764229659] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409815.764183111] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409815.764235070] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409816.764308311] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409816.764362654] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409816.782527509] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409817.333485734] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409817.764216117] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409817.764269418] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409818.764184825] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409818.764234409] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409819.267339971] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409819.267390677] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409819.764203527] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409819.764260024] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.606993302] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.607043196] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.748194354] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409820.764149736] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409820.764189692] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409821.764104533] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409821.764161561] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409822.764151759] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409822.764223735] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409823.764083220] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409823.764136411] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409824.764143506] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409824.764198079] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409824.766423494] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409825.317389150] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409825.764192539] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409825.764233758] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409826.764187190] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409826.764257223] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409827.764097767] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409827.764145818] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409828.024658497] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409828.024714844] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409828.764153389] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409828.764204746] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409829.764194344] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409829.764249919] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409830.764183563] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409830.764231384] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409831.764209764] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409831.764259027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409832.764289690] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409832.764339715] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409833.764213431] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409833.764263245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409834.764203131] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409834.764252324] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409835.764172697] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409835.764224516] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409836.764150921] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409836.764198401] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409837.764182699] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409837.764239306] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409838.340023718] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409838.764146377] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409838.764190791] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409838.891018373] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409839.764202651] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409839.764240242] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409840.083369699] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.083423341] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.764216716] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409840.764281318] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.871145575] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409840.871208274] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409841.764198548] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409841.764251038] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409842.668214952] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409842.764281857] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409842.764335960] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409843.764142655] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409843.764192810] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409844.764157054] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409844.764230965] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409845.764392813] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409845.764455562] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409846.764194145] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409846.764248037] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409847.764208528] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409847.764249636] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409848.764175566] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409848.764228657] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409849.533039064] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409849.764145599] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409849.764197628] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409850.083961123] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409850.764292220] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409850.764342245] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409850.864032152] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409850.864078340] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409851.764195297] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409851.764274027] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409852.764175676] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409852.764232103] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409853.764172091] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409853.764220353] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409854.764177934] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409854.764228691] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409855.764216244] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409855.764271629] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409856.764091563] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409856.764138863] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409857.764163193] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409857.764211815] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409858.764203304] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409858.764254541] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409859.764161922] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409859.764208641] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409860.764203237] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409860.764255276] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409861.069795227] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409861.620866347] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409861.764215221] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409861.764266990] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409862.438528062] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.438587956] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.764335855] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409862.764378456] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.767017921] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.767074939] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409862.962206344] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409863.764169838] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409863.764219502] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409864.764191833] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409864.764242560] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409865.764171424] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409865.764229274] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409866.764185664] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409866.764240809] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409867.764406392] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409867.764454714] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409867.960177944] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409868.511163799] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409868.764178218] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409868.764236378] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409869.255438544] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409869.255499320] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409869.764144008] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409869.764197760] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409870.764189251] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409870.764241730] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409871.764179272] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409871.764257400] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409872.764179042] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409872.764254194] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409873.764170001] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409873.764238240] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409874.764203882] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409874.764273675] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409875.764251384] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409875.764307981] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409876.764219542] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409876.764274557] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409877.764198201] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409877.764267423] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409878.764162437] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409878.764212001] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409879.764204727] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409879.764260693] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409879.948926906] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409880.499862929] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409880.764196880] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409880.764254880] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409881.764169136] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409881.764239410] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409881.983318541] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409882.534276948] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409882.764203731] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409882.764260399] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409883.387738055] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.387793610] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.551063022] [scaled_joint_trajectory_controller]: Received new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.551120781] [scaled_joint_trajectory_controller]: Accepted new action goal -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.760222550] [scaled_joint_trajectory_controller]: Goal reached, success! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409883.764180751] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409883.764220707] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409884.764142597] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409884.764195047] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409885.764155987] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409885.764202947] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409886.764160882] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409886.764204946] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409887.764237674] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409887.764295594] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409888.764190966] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409888.764249117] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409889.363671580] [io_and_status_controller]: Setting speed slider to 50.00%. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409889.764147673] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409889.764195784] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409889.914686842] [io_and_status_controller]: Could not verify that target speed fraction was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409890.764151902] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409890.764221915] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409890.828126416] [io_and_status_controller]: Could not verify that payload was set. (This might happen when using the mocked interface) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409890.828182784] [io_and_status_controller]: Payload has been set successfully -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409891.764203500] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409891.764251812] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409892.764138920] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409892.764187272] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409893.764178169] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409893.764250006] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409894.764175368] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409894.764223350] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [INFO] [1778409895.764221848] [controller_manager]: Received robot description from topic. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ros2_control_node-1] [WARN] [1778409895.764274678] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409762.319032737] [robot_state_publisher]: Robot initialized -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409911.373843058] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_storage.scripts.storage' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer URDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.urdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:Cannot infer SRDF from `/opt/ros/jazzy/share/ur_moveit_config`. -- using config/ur.srdf -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:"File /opt/ros/jazzy/share/ur_moveit_config/config/ur.urdf doesn't exist" -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING:root:The robot description will be loaded from /robot_description topic  -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:47 WARNING  Interaction with robot dashboard disabled. Make sure you know what it means. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Serving Flask app 'arcor2_ur.scripts.ur' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Debug mode: off -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 WARNING: This is a development server. Do not use it in a production deployment. Use a production WSGI server instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on all addresses (0.0.0.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://127.0.0.1:58727 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 * Running on http://172.30.43.138:58727 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Press CTRL+C to quit -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "GET /healthz/ready HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "PUT /system/start HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "GET /system/running HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:42:47] "GET /state/started HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.257718930] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize rclcpp -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.257775468] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node parameters -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.257788693] [moveit_1502777475.moveit.py.cpp_initializer]: Initialize node and executor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.264980653] [moveit_1502777475.moveit.py.cpp_initializer]: Spin separate thread -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.266709778] [moveit_1502777475.moveit.ros.rdf_loader]: Loaded robot model in 0.00161847 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.266735237] [moveit_1502777475.moveit.core.robot_model]: Loading robot model 'ur5e'... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.266743102] [moveit_1502777475.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409772.279139666] [moveit_1502777475.moveit.core.robot_model]: Could not identify parent group for end-effector 'suction' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.281573511] [moveit_1502777475.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'ur_manipulator': 1 1 1 1 1 1 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.334319664] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.334435014] [moveit_1502777475.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.335323071] [moveit_1502777475.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.335873668] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.335886742] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.336014686] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.336392794] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.336480110] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting planning scene monitor -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337085761] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337097563] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337542359] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.337993165] [moveit_1502777475.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409772.338242710] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409772.338258289] [moveit_1502777475.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.820442676] [moveit_1502777475.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.821083354] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822069167] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822079546] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822369427] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822385167] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822410565] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822416186] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.822426466] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.823013311] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.825261203] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.825276572] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.827036275] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.827049400] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.827853978] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.856359461] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.859791132] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.859954893] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.859977827] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409772.860833573] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:52 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:54 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:55 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:55 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:57 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:42:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:43:09] "PUT /state/start HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:43:09] "GET /joints HTTP/1.1" 200 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 127.0.0.1 - - [10/May/2026 12:43:10] "PUT /collisions/cylinder?radius=0.1&height=0.2&cylinderId=Cyl1 HTTP/1.1" 204 - -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:10 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:10 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:11 INFO  Generated 20 grasp options for object Cyl1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  IK accepted: 20 grasp options. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:12 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:14 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:14 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:15 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:15 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153443391] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153563640] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153577316] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.153595170] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409796.153662067] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.154085602] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.154206211] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.169621863] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.170072870] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.170416763] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216943760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216965231] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216983004] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.216989677] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.217007110] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218558112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218597668] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218622855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.218775075] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.219582153] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409796.219600488] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:16 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409797.720152829] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409797.724501172] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:17 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:18 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:18 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:19 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:19 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:20 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:20 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:25 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430164123] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430238374] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430252491] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430265947] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409805.430344977] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430586536] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409805.430635269] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.433369314] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.433788250] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.433897812] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.434094120] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409815.434251880] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003569 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.442111916] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409815.442221494] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:35 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:35 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:36 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:36 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:38 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:38 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584147221] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584203178] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584211964] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584223005] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409820.584299541] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584518948] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.584598829] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.597357623] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.597428688] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.597522907] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:40 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604442585] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604467592] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604485016] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604493532] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.604512668] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606562112] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606591648] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606612728] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.606725563] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.607298807] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.607312493] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.757777293] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409820.762481005] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:41 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:41 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:43 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:47 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:48 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309517858] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309572281] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309580797] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309592019] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409828.309701827] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.309951271] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409828.310044769] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.313605646] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.313763095] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.314096699] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.314217483] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409838.314339981] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004222 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.335261686] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409838.335320918] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:58 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:59 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:43:59 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.334937564] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.334986878] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.334994031] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.335004962] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409840.335072430] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.335303730] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.335344007] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.361554961] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.362156544] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.362570550] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:00 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868328733] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868368077] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868408855] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868422000] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.868454421] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870634349] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870682330] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870704953] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.870838467] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.871434003] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409840.871448691] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409842.671872612] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409842.676467122] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:07 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:07 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:08 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:08 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:09 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:09 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:11 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:11 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:11 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005531569] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005608716] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005620558] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005633563] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409851.005712773] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.005958300] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409851.006008856] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008170904] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008356352] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008610689] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.008726895] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409861.008897064] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.002831 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.027596805] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409861.027655586] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:21 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724713760] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724773343] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724782410] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.724793952] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409862.724870548] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.725097599] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.725148887] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.741512678] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.741581709] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.741639619] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765078426] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765101149] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765115166] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765122760] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.765139312] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766605713] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766639918] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766661479] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.766773902] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.767293940] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.767308498] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:22 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.967715412] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409862.972475057] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:24 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:25 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:25 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:26 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:26 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:27 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:27 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:28 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:28 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:29 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:29 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:29 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903117283] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903168130] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903175123] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903184341] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409869.903280754] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903521070] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409869.903615410] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.906350239] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.906886684] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.907037235] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.907146348] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409879.907300711] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003617 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.930359365] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409879.930435400] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:39 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:39 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:42 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:43 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544616182] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544667620] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544674794] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544684552] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409883.544758092] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.544983991] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.545065927] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.548860186] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.548946149] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549026222] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:43 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549221883] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549240869] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549254145] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549261188] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.549276377] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550556860] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550584162] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550612315] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.550723767] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.551306820] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.551322309] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.801675456] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409883.806462571] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:44 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:44 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:45 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:45 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:46 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:48 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:48 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:49 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:49 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:50 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:50 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:51 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:51 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:44:51 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023659043] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023709839] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023716782] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.023726441] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409891.023790152] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.024005240] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409891.024046759] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.026011506] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.027183142] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.027674426] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.027763870] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409901.027877612] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.003787 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.030747966] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409901.030807449] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:01 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478695882] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478746709] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478753822] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.478763250] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409902.478825398] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.479049083] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.479089971] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.491502647] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.491578932] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.491652091] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Executing plan -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658462590] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658489952] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658507485] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658516202] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.658536400] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660578857] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660618292] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660643460] [moveit_1502777475.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.660760553] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to scaled_joint_trajectory_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.661270336] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.661281607] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:02 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.861545943] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'scaled_joint_trajectory_controller' successfully finished -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409902.866470690] [moveit_1502777475.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:03 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:03 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:04 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:04 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:05 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:05 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:06 INFO  Robot mode: 7 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:06 INFO  Program running: True -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:07 INFO  Planning trajectory -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578555476] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578604820] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578613286] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578622273] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409907.578684071] [moveit_1502777475.moveit.planners.ompl.planning_context_manager]: Cannot find planning configuration for group 'ur_manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578887607] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409907.578929567] [moveit_py]: Calling Planner 'OMPL' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409911.502946530] [rclcpp]: signal_handler(signum=15) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.582781701] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.582860536] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.583501359] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.583585333] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:265 - ur_manipulator/ur_manipulator: Unable to sample any valid states for goal tree -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [WARN] [1778409917.583696751] [moveit_1502777475.moveit.planners.ompl.planner_manager]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 10.004731 seconds -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.604220980] [moveit_1502777475.moveit.planners.ompl.model_based_planning_context]: Unable to solve the planning problem -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [ERROR] [1778409917.604280282] [moveit_py]: Planner 'OMPL' failed with error code FAILURE -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:43 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1173, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 result = runtime.attach_object( -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1029, in attach_object -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral(f"Unable to attach object {object_id}.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Unable to attach object Cyl1. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:43 ERROR  ROS worker command failed. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 1208, in ros_worker_main -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 conn.send({"status": "ok", "result": result}) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 206, in send -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send_bytes(_ForkingPickler.dumps(obj)) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 427, in _send_bytes -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._send(header + buf) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/usr/lib/python3.12/multiprocessing/connection.py", line 384, in _send -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 n = write(self._handle, buf) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 BrokenPipeError: [Errno 32] Broken pipe -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:58 ERROR  Failed to switch controllers off during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 515, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._switch_freedrive_controller(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 574, in _switch_freedrive_controller -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 active = self._active_controllers() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 565, in _active_controllers -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 resp = wait_for_future(self._list_controllers_client.call_async(req), timeout_sec=15.0) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 179, in wait_for_future -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 raise UrGeneral("Service call timed out.") -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 arcor2_ur.exceptions.UrGeneral: Service call timed out. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 >>> [rcutils|error_handling.c:108] rcutils_set_error_state() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 This error state is being overwritten: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'Handle's typesupport identifier (rosidl_typesupport_cpp) is not supported by this library, at ./src/type_support_dispatch.hpp:114' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 with this new error message: -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 'publisher's context is invalid, at ./src/rcl/publisher.c:423' -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rcutils_reset_error() should be called after error handling to avoid this. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 <<< -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:58 ERROR  Failed to clear freedrive mode during shutdown. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 520, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.node.set_freedrive_mode(False) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 408, in set_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self._publish_freedrive_mode(enabled) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 421, in _publish_freedrive_mode -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.freedrive_mode_pub.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/publisher.py", line 70, in publish -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__publisher.publish(msg) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:423 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 2026-05-10 12:45:58 ERROR  Failed to shutdown rclpy. -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 Traceback (most recent call last): -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/tmp/.cache/pex/venvs/3/f09b51a488381181906d97d6dd9e59e18ce3db2b/779eb2cc0ca9e2fdd204774cbc41848e4e7c5055/lib/python3.12/site-packages/arcor2_ur/scripts/ros_worker.py", line 556, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 _shutdown(context=context) -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 self.__context.shutdown() -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 -ERROR  arcor2_arserver.tests.testutils:testutils.py:33 [INFO] [1778409958.792785146] [moveit_1502777475.moveit.ros.moveit_cpp]: Deleting MoveItCpp -=============================== warnings summary =============================== -src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:13 - src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:13: PytestUnknownMarkWarning: Unknown pytest.mark.timeout - is this a typo? You can register custom marks to avoid this warning - for details, see https://docs.pytest.org/en/stable/how-to/mark.html - @pytest.mark.timeout(321) - --- Docs: https://docs.pytest.org/en/stable/how-to/capture-warnings.html -- generated xml file: src.python.arcor2_ur.tests.pick_and_place.test_pick_and_place_cylinder_by_position.py@tests.xml - -=========================== short test summary info ============================ -FAILED src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py::test_pick_and_place_cylinder_by_position[ur5e] - arcor2_web.rest.RestException: Catastrophic system error. -=================== 1 failed, 1 warning in 202.00s (0:03:21) =================== - - - -✓ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py:../tests succeeded in 34.46s. -+ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py:../tests succeeded after 3 attempts in 35.13s. -✓ src/python/arcor2_ur/tests/test_interaction.py succeeded in 58.78s (cached locally). -✓ src/python/arcor2_ur/tests/test_ur.py succeeded in 3.44s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_id.py:../tests failed after 5 attempts in 100.82s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_box_by_position.py:../tests failed after 5 attempts in 100.14s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_id.py:../tests failed after 5 attempts in 204.06s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_cylinder_by_position.py:../tests failed after 5 attempts in 202.80s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_id.py:../tests failed after 5 attempts in 49.36s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_mesh_by_position.py:../tests failed after 5 attempts in 82.90s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_rotated_box.py:../tests failed after 5 attempts in 142.73s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_id.py:../tests failed after 5 attempts in 204.15s. -✕ src/python/arcor2_ur/tests/pick_and_place/test_pick_and_place_sphere_by_position.py:../tests failed after 5 attempts in 177.11s. -✕ src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py:../tests failed after 5 attempts in 44.37s. -✕ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py:../tests failed after 5 attempts in 44.09s. -✕ src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py:../tests failed after 5 attempts in 46.75s. -✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py:../tests failed after 5 attempts in 45.30s. -✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py:../tests failed after 5 attempts in 51.39s. -✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py:../tests failed after 5 attempts in 70.44s. -✕ src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py:../tests failed after 5 attempts in 50.15s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py:../tests failed after 5 attempts in 34.25s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py:../tests failed after 5 attempts in 33.75s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py:../tests failed after 5 attempts in 31.82s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py:../tests failed after 5 attempts in 33.69s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py:../tests failed after 5 attempts in 33.48s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py:../tests failed after 5 attempts in 33.14s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py:../tests failed after 5 attempts in 33.63s. -✕ src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py:../tests failed after 5 attempts in 34.05s. -✕ src/python/arcor2_ur/tests/test_suck_effector.py failed after 5 attempts in 10.84s. diff --git a/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py index b3770641..e47573f4 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py +++ b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box, Cylinder, Sphere from arcor2_object_types.abstract import GraspableState @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_graspable(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py index 347accb8..7dc5a6a6 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_box.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_scene_data import scene_service @@ -9,7 +7,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_overhead_box(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py index 86cf082e..1875adf3 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_cylinder.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder from arcor2_scene_data import scene_service @@ -9,7 +7,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_overhead_cylinder(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py index 99ea5484..ef2d2204 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_mesh.py @@ -1,8 +1,6 @@ import time from pathlib import Path -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh from arcor2_scene_data import scene_service @@ -14,7 +12,6 @@ MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(400) def test_overhead_mesh(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" diff --git a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py index 6a016848..d9bde171 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py +++ b/src/python/arcor2_ur/tests/test_collision/test_overhead_obstacle_sphere.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere from arcor2_scene_data import scene_service @@ -9,7 +7,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_overhead_sphere(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py index 1a45c9ff..b3755629 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_box.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_scene_data import scene_service @@ -9,7 +7,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_side_box(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py index 3846c25f..a5aaa71c 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_cylinder.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder from arcor2_scene_data import scene_service @@ -9,7 +7,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_side_cylinder(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py index 29dd639d..452a5858 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_mesh.py @@ -1,8 +1,6 @@ import time from pathlib import Path -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh from arcor2_scene_data import scene_service @@ -14,7 +12,6 @@ MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(400) def test_side_mesh(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" diff --git a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py index 03851788..d57c26c8 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py +++ b/src/python/arcor2_ur/tests/test_collision/test_side_obstacle_sphere.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere from arcor2_scene_data import scene_service @@ -9,7 +7,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_side_sphere(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() diff --git a/src/python/arcor2_ur/tests/test_interaction.py b/src/python/arcor2_ur/tests/test_interaction.py index c9023493..4106d635 100644 --- a/src/python/arcor2_ur/tests/test_interaction.py +++ b/src/python/arcor2_ur/tests/test_interaction.py @@ -10,7 +10,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(60) def test_basics(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url box = Box("UniqueBoxId", 0.1, 0.1, 0.1) diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py new file mode 100644 index 00000000..828c94e5 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py @@ -0,0 +1,53 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_and_place_side_obstacle(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + box = Box("Box2", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box3", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(-0.3, 0.0, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box4", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, 0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + box = Box("Box5", 0.2, 0.2, 0.2) + scene_service.upsert_collision(box, Pose(Position(0.0, -0.3, 1.0), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), 0.3) + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py new file mode 100644 index 00000000..3a3551e2 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py @@ -0,0 +1,38 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_and_place_side_obstacle(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + box = Box("Box2", 0.2, 0.2, 0.95) + scene_service.upsert_collision(box, Pose(Position(0.5, 0.5, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + ot.move_to_pose("", Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), 0.3) + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py new file mode 100644 index 00000000..d06664e7 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py @@ -0,0 +1,42 @@ +import time + +import pytest + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2.exceptions import Arcor2Exception +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_and_move_wall(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + box = Box("Wall1", 0.1, 2.0, 1.0) + scene_service.upsert_collision(box, Pose(Position(0.4, 0.0, 0.5), Orientation(0, 0, 0, 1))) + assert box.id in scene_service.collision_ids() + time.sleep(1) + + with pytest.raises(Arcor2Exception): + ot.move_to_pose("", Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), 0.3) + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py index 523fdc59..2883f57e 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py index fcd147de..d86e90d7 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_and_place_box_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_box_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py new file mode 100644 index 00000000..f2cb2108 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py @@ -0,0 +1,37 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_and_place_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + X = 0.0 + Y = 0.5 + Z = 0.1 + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.LEFT]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) + + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py index 6d180fba..2698ab45 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py index 2a5d4559..d9f870c4 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py index 584efaa5..62992462 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py @@ -1,8 +1,6 @@ import time from pathlib import Path -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -15,7 +13,6 @@ MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(321) def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" @@ -42,14 +39,14 @@ def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py index 74df8d77..cafa1a99 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py @@ -1,8 +1,6 @@ import time from pathlib import Path -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -15,7 +13,6 @@ MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(321) def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" @@ -42,13 +39,13 @@ def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py index cbc78399..afd48c55 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0.5, 0.5, 0.5, 0)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py index 7dc450a3..4f1a1034 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py index 01aa1258..2aef9582 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,14 +24,14 @@ def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED ot.place_object(Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1))) - assert Position(Y, X, Z).distance(scene_service.graspable_position(object.id)) < 0.03 - assert scene_service.graspable_state(object.id) == GraspableState.WORLD + assert Position(Y, X, Z).distance(ot.graspable_position(object.id)) < 0.03 + assert ot.graspable_state(object.id) == GraspableState.WORLD scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py new file mode 100644 index 00000000..7c3b9557 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py @@ -0,0 +1,32 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py new file mode 100644 index 00000000..a98ebc81 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py @@ -0,0 +1,32 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.BACK]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py new file mode 100644 index 00000000..c6776b61 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py @@ -0,0 +1,32 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.7 + Z = 0.7 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.BOTTOM]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py index abdb15e6..e1d502fa 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(400) def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,9 +24,9 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py index d2373802..ebc4c363 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_up_box_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,9 +24,9 @@ def test_pick_up_box_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py new file mode 100644 index 00000000..0e4249c6 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py @@ -0,0 +1,32 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.FRONT]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py new file mode 100644 index 00000000..f3f604b4 --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py @@ -0,0 +1,32 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.LEFT]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py new file mode 100644 index 00000000..792480ec --- /dev/null +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py @@ -0,0 +1,32 @@ +import time + +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition +from arcor2_scene_data import scene_service +from arcor2_ur.object_types.ur5e import Ur5e, UrSettings +from arcor2_ur.tests.conftest import Urls + + +def test_pick_up_box_by_id(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + scene_service.start() + assert scene_service.started() + + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) + assert len(ot.robot_joints()) == 6 + + X = 0.0 + Y = 0.5 + Z = 0.1 + + object = Box("Box1", 0.2, 0.2, 0.2) + scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) + time.sleep(1) + + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.RIGHT]) + + assert ot.graspable_state(object.id) == GraspableState.ATTACHED + + scene_service.delete_all_collisions() + ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py index 5a999583..5e5e6b30 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_up_cylinder_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,9 +24,9 @@ def test_pick_up_cylinder_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py index f526d044..b50e9786 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_up_cylinder_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,9 +24,9 @@ def test_pick_up_cylinder_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py index 83e58e0d..eaf7df15 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py @@ -1,8 +1,6 @@ import time from pathlib import Path -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -15,7 +13,6 @@ MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(321) def test_pick_up_mesh_by_id(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" @@ -42,9 +39,9 @@ def test_pick_up_mesh_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py index 713b184b..ae2eaa6f 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py @@ -1,8 +1,6 @@ import time from pathlib import Path -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -15,7 +13,6 @@ MESH_ASSET_ID = "triangle_block.stl" -@pytest.mark.timeout(321) def test_pick_up_mesh_by_position(start_processes: Urls) -> None: assert MESH_PATH.is_file(), f"Test mesh file does not exist: {MESH_PATH}" @@ -42,9 +39,9 @@ def test_pick_up_mesh_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py index 5119378b..48f28d7e 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_up_sphere_by_id(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,9 +24,9 @@ def test_pick_up_sphere_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py index a627420a..b69245a6 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py @@ -1,7 +1,5 @@ import time -import pytest - from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition @@ -10,7 +8,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(321) def test_pick_up_sphere_by_position(start_processes: Urls) -> None: scene_service.URL = start_processes.robot_url scene_service.start() @@ -27,9 +24,9 @@ def test_pick_up_sphere_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) - assert scene_service.graspable_state(object.id) == GraspableState.ATTACHED + assert ot.graspable_state(object.id) == GraspableState.ATTACHED scene_service.delete_all_collisions() ot.cleanup() diff --git a/src/python/arcor2_ur/tests/test_suck_effector.py b/src/python/arcor2_ur/tests/test_suck_effector.py index d3cbf831..89360fba 100644 --- a/src/python/arcor2_ur/tests/test_suck_effector.py +++ b/src/python/arcor2_ur/tests/test_suck_effector.py @@ -1,5 +1,3 @@ -import pytest - from arcor2.data.common import Pose from arcor2_scene_data import scene_service from arcor2_ur.object_types.ur5e import Ur5e, UrSettings @@ -7,7 +5,6 @@ from arcor2_ur.tests.conftest import Urls -@pytest.mark.timeout(60) def test_suck_effector(start_processes: Urls) -> None: robot_description = load_robot_description() From 2ec83e38d010098a5f379c01e1430f9a28dfb014 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Tue, 12 May 2026 01:30:37 +0200 Subject: [PATCH 10/11] feat: update UR demo stack to 1.8.0 --- compose-files/ur-demo/docker-compose.yml | 21 +- src/docker/arcor2_arserver/BUILD | 2 +- src/docker/arcor2_execution/BUILD | 2 +- src/docker/arcor2_scene/BUILD | 2 +- .../arcor2_upload_builtin_objects/BUILD | 2 +- src/docker/arcor2_ur/BUILD | 2 +- src/docker/arcor2_ur/Dockerfile | 1 + src/docker/arcor2_ur_ot/BUILD | 2 +- src/python/arcor2/VERSION | 2 +- src/python/arcor2_object_types/VERSION | 2 +- src/python/arcor2_scene/VERSION | 2 +- src/python/arcor2_scene_data/VERSION | 2 +- src/python/arcor2_ur/VERSION | 2 +- src/python/arcor2_ur/scripts/ur.py | 227 +++++++++++++++--- 14 files changed, 217 insertions(+), 54 deletions(-) diff --git a/compose-files/ur-demo/docker-compose.yml b/compose-files/ur-demo/docker-compose.yml index 33f38923..30e0b9f9 100644 --- a/compose-files/ur-demo/docker-compose.yml +++ b/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: diff --git a/src/docker/arcor2_arserver/BUILD b/src/docker/arcor2_arserver/BUILD index dc650f02..5940ea54 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.4.0"] + name="arcor2_arserver", repository="arcor2/arcor2_arserver", dependencies=[":start.sh"], image_tags=["1.8.0"] ) diff --git a/src/docker/arcor2_execution/BUILD b/src/docker/arcor2_execution/BUILD index ab18237f..10dcdf3a 100644 --- a/src/docker/arcor2_execution/BUILD +++ b/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"]) diff --git a/src/docker/arcor2_scene/BUILD b/src/docker/arcor2_scene/BUILD index 05625c89..8839d116 100644 --- a/src/docker/arcor2_scene/BUILD +++ b/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"]) diff --git a/src/docker/arcor2_upload_builtin_objects/BUILD b/src/docker/arcor2_upload_builtin_objects/BUILD index 79472ed2..8f9499e9 100644 --- a/src/docker/arcor2_upload_builtin_objects/BUILD +++ b/src/docker/arcor2_upload_builtin_objects/BUILD @@ -1,3 +1,3 @@ docker_image( - name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["2.0.0"] + name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["1.8.0"] ) diff --git a/src/docker/arcor2_ur/BUILD b/src/docker/arcor2_ur/BUILD index 153e5de9..641adb79 100644 --- a/src/docker/arcor2_ur/BUILD +++ b/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.7.0"]) +docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.8.0"]) diff --git a/src/docker/arcor2_ur/Dockerfile b/src/docker/arcor2_ur/Dockerfile index c7146f90..f4e5d70e 100644 --- a/src/docker/arcor2_ur/Dockerfile +++ b/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 \ diff --git a/src/docker/arcor2_ur_ot/BUILD b/src/docker/arcor2_ur_ot/BUILD index 2c9556dd..cc1ed7bf 100644 --- a/src/docker/arcor2_ur_ot/BUILD +++ b/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"]) diff --git a/src/python/arcor2/VERSION b/src/python/arcor2/VERSION index 359a5b95..27f9cd32 100644 --- a/src/python/arcor2/VERSION +++ b/src/python/arcor2/VERSION @@ -1 +1 @@ -2.0.0 \ No newline at end of file +1.8.0 diff --git a/src/python/arcor2_object_types/VERSION b/src/python/arcor2_object_types/VERSION index 227cea21..27f9cd32 100644 --- a/src/python/arcor2_object_types/VERSION +++ b/src/python/arcor2_object_types/VERSION @@ -1 +1 @@ -2.0.0 +1.8.0 diff --git a/src/python/arcor2_scene/VERSION b/src/python/arcor2_scene/VERSION index 1cc5f657..27f9cd32 100644 --- a/src/python/arcor2_scene/VERSION +++ b/src/python/arcor2_scene/VERSION @@ -1 +1 @@ -1.1.0 \ No newline at end of file +1.8.0 diff --git a/src/python/arcor2_scene_data/VERSION b/src/python/arcor2_scene_data/VERSION index 3eefcb9d..27f9cd32 100644 --- a/src/python/arcor2_scene_data/VERSION +++ b/src/python/arcor2_scene_data/VERSION @@ -1 +1 @@ -1.0.0 +1.8.0 diff --git a/src/python/arcor2_ur/VERSION b/src/python/arcor2_ur/VERSION index 9dbb0c00..27f9cd32 100644 --- a/src/python/arcor2_ur/VERSION +++ b/src/python/arcor2_ur/VERSION @@ -1 +1 @@ -1.7.0 \ No newline at end of file +1.8.0 diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index b8f37f6b..187dc197 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -141,7 +141,7 @@ def put_start() -> RespT: content: application/json: schema: - $ref: Pose + $ref: '#/components/schemas/Pose' responses: 204: description: Ok @@ -267,20 +267,30 @@ def pick_up_object_by_position() -> RespT: - radius properties: position: - $ref: Position + $ref: '#/components/schemas/Position' radius: type: number format: float effector_type: type: string - default: suck + enum: + - SUCK + default: SUCK grasp_positions: type: array items: type: string + enum: + - TOP + - RIGHT + - LEFT + - FRONT + - BACK + - BOTTOM + - ALL object_type_name: type: string - default: none + description: Optional model type filter. velocity: type: number format: float @@ -413,11 +423,21 @@ def pick_up_object_by_id() -> RespT: type: string effector_type: type: string - default: suck + enum: + - SUCK + default: SUCK grasp_positions: - type: array - items: - type: string + type: array + items: + type: string + enum: + - TOP + - RIGHT + - LEFT + - FRONT + - BACK + - BOTTOM + - ALL velocity: type: number format: float @@ -518,10 +538,25 @@ def place_object() -> RespT: properties: object_id: type: string + description: Optional object ID. effector_type: type: string + enum: + - SUCK + default: SUCK pose: - $ref: Pose + $ref: '#/components/schemas/Pose' + velocity: + type: number + format: float + default: 50.0 + payload: + type: number + format: float + default: 0.0 + safe: + type: boolean + default: true responses: 204: description: Ok @@ -564,7 +599,7 @@ def put_box() -> RespT: put: tags: - Collisions - description: Add or update collision box. + description: Add or update collision box. Use metadata.object_type=graspable for graspable objects. parameters: - name: boxId in: query @@ -588,10 +623,41 @@ def put_box() -> RespT: type: number format: float requestBody: - content: + required: true + content: application/json: - schema: - $ref: Pose + schema: + type: object + required: + - pose + properties: + pose: + $ref: '#/components/schemas/Pose' + metadata: + type: object + description: Optional metadata for graspable object support. + properties: + object_type: + type: string + enum: + - graspable + state: + type: string + enum: + - WORLD + - RESERVED + - HIDDEN + - ATTACHED + source: + type: string + enum: + - CAMERA + - FIXED + - OTHER + stamp: + type: string + format: date-time + additionalProperties: true responses: 204: description: Ok @@ -623,7 +689,7 @@ def put_sphere() -> RespT: put: tags: - Collisions - description: Add or update collision sphere. + description: Add or update collision sphere. Use metadata.object_type=graspable for graspable objects. parameters: - name: sphereId in: query @@ -637,10 +703,41 @@ def put_sphere() -> RespT: type: number format: float requestBody: - content: + required: true + content: application/json: - schema: - $ref: Pose + schema: + type: object + required: + - pose + properties: + pose: + $ref: '#/components/schemas/Pose' + metadata: + type: object + description: Optional metadata for graspable object support. + properties: + object_type: + type: string + enum: + - graspable + state: + type: string + enum: + - WORLD + - RESERVED + - HIDDEN + - ATTACHED + source: + type: string + enum: + - CAMERA + - FIXED + - OTHER + stamp: + type: string + format: date-time + additionalProperties: true responses: 204: description: Ok @@ -672,7 +769,7 @@ def put_cylinder() -> RespT: put: tags: - Collisions - description: Add or update collision cylinder. + description: Add or update collision cylinder. Use metadata.object_type=graspable for graspable objects. parameters: - name: cylinderId in: query @@ -691,17 +788,44 @@ def put_cylinder() -> RespT: type: number format: float requestBody: - content: + required: true + content: application/json: - schema: - $ref: Pose + schema: + type: object + required: + - pose + properties: + pose: + $ref: '#/components/schemas/Pose' + metadata: + type: object + description: Optional metadata for graspable object support. + properties: + object_type: + type: string + enum: + - graspable + state: + type: string + enum: + - WORLD + - RESERVED + - HIDDEN + - ATTACHED + source: + type: string + enum: + - CAMERA + - FIXED + - OTHER + stamp: + type: string + format: date-time + additionalProperties: true responses: - 200: + 204: description: Ok - content: - application/json: - schema: - type: string 500: description: "Error types: **General**, **SceneGeneral**." content: @@ -730,7 +854,7 @@ def put_mesh() -> RespT: put: tags: - Collisions - description: Add or update collision mesh. + description: Add or update collision mesh. Use metadata.object_type=graspable for graspable objects. parameters: - name: meshId in: query @@ -761,10 +885,41 @@ def put_mesh() -> RespT: format: float default: 1.0 requestBody: - content: + required: true + content: application/json: - schema: - $ref: Pose + schema: + type: object + required: + - pose + properties: + pose: + $ref: '#/components/schemas/Pose' + metadata: + type: object + description: Optional metadata for graspable object support. + properties: + object_type: + type: string + enum: + - graspable + state: + type: string + enum: + - WORLD + - RESERVED + - HIDDEN + - ATTACHED + source: + type: string + enum: + - CAMERA + - FIXED + - OTHER + stamp: + type: string + format: date-time + additionalProperties: true responses: 204: description: Ok @@ -880,7 +1035,7 @@ def get_joints() -> RespT: schema: type: array items: - $ref: Joint + $ref: '#/components/schemas/Joint' 500: description: "Error types: **General**, **StartError**." content: @@ -905,7 +1060,7 @@ def put_ik() -> RespT: content: application/json: schema: - $ref: InverseKinematicsRequest + $ref: '#/components/schemas/InverseKinematicsRequest' responses: 200: description: Ok @@ -914,7 +1069,7 @@ def put_ik() -> RespT: schema: type: array items: - $ref: Joint + $ref: '#/components/schemas/Joint' 500: description: "Error types: **General**, **DobotGeneral**, **StartError**." content: @@ -1007,7 +1162,7 @@ def get_eef_pose() -> RespT: content: application/json: schema: - $ref: Pose + $ref: '#/components/schemas/Pose' 500: description: "Error types: **General**, **StartError**." content: @@ -1055,7 +1210,7 @@ def put_eef_pose() -> RespT: content: application/json: schema: - $ref: Pose + $ref: '#/components/schemas/Pose' responses: 200: description: Ok @@ -1137,7 +1292,7 @@ def get_vacuum() -> RespT: content: application/json: schema: - $ref: Vacuum + $ref: '#/components/schemas/Vacuum' 500: description: "Error types: **General**, **StartError**." content: From b13090a58d07ae0aae396b383ff47b7aa745afd3 Mon Sep 17 00:00:00 2001 From: Simon Kadnar Date: Fri, 15 May 2026 00:20:44 +0200 Subject: [PATCH 11/11] feat: update UR Docker setup and move shared types --- .../arcor2_upload_builtin_objects/BUILD | 4 +- src/docker/arcor2_ur/BUILD | 13 ++- src/docker/arcor2_ur/Dockerfile | 1 + src/docker/arcor2_ur/start.sh | 6 + src/python/arcor2/BUILD | 2 +- src/python/arcor2_build/scripts/BUILD | 5 +- src/python/arcor2_calibration/BUILD | 2 +- src/python/arcor2_object_types/abstract.py | 59 +--------- src/python/arcor2_scene/BUILD | 2 +- src/python/arcor2_ur/common.py | 56 ++++++++++ src/python/arcor2_ur/object_types/ur5e.py | 27 ++--- src/python/arcor2_ur/scripts/grasp_planner.py | 21 ++-- src/python/arcor2_ur/scripts/ros_worker.py | 46 +++----- src/python/arcor2_ur/scripts/ur.py | 103 +++++++----------- .../test_collision/test_obstacle_graspable.py | 2 +- .../arcor2_ur/tests/test_interaction.py | 2 +- .../test_pick_and_move_overhead_obstacle.py | 4 +- .../test_pick_and_move_side_obstacle.py | 4 +- .../test_pick_and_move_wall.py | 4 +- .../test_pick_and_place_box_by_id.py | 4 +- .../test_pick_and_place_box_by_position.py | 4 +- .../test_pick_and_place_box_left.py | 4 +- .../test_pick_and_place_cylinder_by_id.py | 4 +- ...est_pick_and_place_cylinder_by_position.py | 4 +- .../test_pick_and_place_mesh_by_id.py | 4 +- .../test_pick_and_place_mesh_by_position.py | 4 +- .../test_pick_and_place_rotated_box.py | 4 +- .../test_pick_and_place_sphere_by_id.py | 4 +- .../test_pick_and_place_sphere_by_position.py | 4 +- .../test_pick_up/test_pick_up_box_all.py | 4 +- .../test_pick_up/test_pick_up_box_back.py | 4 +- .../test_pick_up/test_pick_up_box_bottom.py | 4 +- .../test_pick_up/test_pick_up_box_by_id.py | 4 +- .../test_pick_up_box_by_position.py | 4 +- .../test_pick_up/test_pick_up_box_front.py | 4 +- .../test_pick_up/test_pick_up_box_left.py | 4 +- .../test_pick_up/test_pick_up_box_right.py | 4 +- .../test_pick_up_cylinder_by_id.py | 4 +- .../test_pick_up_cylinder_by_position.py | 4 +- .../test_pick_up/test_pick_up_mesh_by_id.py | 4 +- .../test_pick_up_mesh_by_position.py | 4 +- .../test_pick_up/test_pick_up_sphere_by_id.py | 4 +- .../test_pick_up_sphere_by_position.py | 4 +- .../arcor2_ur/tests/test_suck_effector.py | 2 +- 44 files changed, 216 insertions(+), 245 deletions(-) diff --git a/src/docker/arcor2_upload_builtin_objects/BUILD b/src/docker/arcor2_upload_builtin_objects/BUILD index 8f9499e9..3e0adf93 100644 --- a/src/docker/arcor2_upload_builtin_objects/BUILD +++ b/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=["1.8.0"] -) +docker_image(name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["1.8.0"]) diff --git a/src/docker/arcor2_ur/BUILD b/src/docker/arcor2_ur/BUILD index 641adb79..c0f9537e 100644 --- a/src/docker/arcor2_ur/BUILD +++ b/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.8.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"], +) diff --git a/src/docker/arcor2_ur/Dockerfile b/src/docker/arcor2_ur/Dockerfile index f4e5d70e..e72fdbed 100644 --- a/src/docker/arcor2_ur/Dockerfile +++ b/src/docker/arcor2_ur/Dockerfile @@ -55,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"] diff --git a/src/docker/arcor2_ur/start.sh b/src/docker/arcor2_ur/start.sh index e4ec1ced..8ef1b0ee 100755 --- a/src/docker/arcor2_ur/start.sh +++ b/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 diff --git a/src/python/arcor2/BUILD b/src/python/arcor2/BUILD index 8d927e09..a8e3ace1 100644 --- a/src/python/arcor2/BUILD +++ b/src/python/arcor2/BUILD @@ -6,5 +6,5 @@ arcor2_python_distribution( "src/python/arcor2/data/rpc", "src/python/arcor2/exceptions", "src/python/arcor2/source", - ] + ], ) diff --git a/src/python/arcor2_build/scripts/BUILD b/src/python/arcor2_build/scripts/BUILD index 586235f6..a65b567a 100644 --- a/src/python/arcor2_build/scripts/BUILD +++ b/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", + ], ) diff --git a/src/python/arcor2_calibration/BUILD b/src/python/arcor2_calibration/BUILD index c0ce6603..de15e019 100644 --- a/src/python/arcor2_calibration/BUILD +++ b/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"}, ) diff --git a/src/python/arcor2_object_types/abstract.py b/src/python/arcor2_object_types/abstract.py index 7a1fe199..bab81db7 100644 --- a/src/python/arcor2_object_types/abstract.py +++ b/src/python/arcor2_object_types/abstract.py @@ -9,12 +9,13 @@ from arcor2 import CancelDict, DynamicParamDict from arcor2.data.camera import CameraParameters -from arcor2.data.common import ActionMetadata, Joint, Pose, SceneObject, StrEnum +from arcor2.data.common import ActionMetadata, Joint, Pose, SceneObject from arcor2.data.object_type import Models, PrimitiveModels from arcor2.data.robot import RobotType 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): @@ -184,62 +185,6 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None: set_enabled.__action__ = ActionMetadata() # type: ignore -class GraspPosition(StrEnum): - TOP = "TOP" - RIGHT = "RIGHT" - LEFT = "LEFT" - FRONT = "FRONT" - BACK = "BACK" - BOTTOM = "BOTTOM" - ALL = "ALL" - - -class EffectorType(StrEnum): - SUCK = "SUCK" - - -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. - """ - - WORLD = "WORLD" - RESERVED = "RESERVED" - HIDDEN = "HIDDEN" - ATTACHED = "ATTACHED" - - -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" - # TODO: remove - OTHER = "OTHER" - - def _utc_now_iso() -> str: """Return the current UTC timestamp in ISO 8601 format.""" return datetime.now(timezone.utc).isoformat() diff --git a/src/python/arcor2_scene/BUILD b/src/python/arcor2_scene/BUILD index 2c872bc4..5810d059 100644 --- a/src/python/arcor2_scene/BUILD +++ b/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", - } + }, ) diff --git a/src/python/arcor2_ur/common.py b/src/python/arcor2_ur/common.py index 1fc2df48..5a30d68a 100644 --- a/src/python/arcor2_ur/common.py +++ b/src/python/arcor2_ur/common.py @@ -5,9 +5,65 @@ from flask import request from arcor2.data import common, object_type +from arcor2.data.common import StrEnum from arcor2_ur.exceptions import UrGeneral +class GraspPosition(StrEnum): + TOP = "TOP" + RIGHT = "RIGHT" + LEFT = "LEFT" + FRONT = "FRONT" + BACK = "BACK" + BOTTOM = "BOTTOM" + ALL = "ALL" + + +class EffectorType(StrEnum): + SUCK = "SUCK" + + +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. + """ + + WORLD = "WORLD" + RESERVED = "RESERVED" + HIDDEN = "HIDDEN" + ATTACHED = "ATTACHED" + + +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" + + @dataclass class CollisionSceneObject: model: object_type.Models diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index e95440d3..4c3ecb21 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -1,5 +1,5 @@ import time -from dataclasses import dataclass, field +from dataclasses import dataclass from typing import cast from dataclasses_jsonschema import JsonSchemaMixin @@ -7,7 +7,8 @@ from arcor2.data import object_type from arcor2.data.common import ActionMetadata, Joint, Pose, Position, StrEnum from arcor2.data.robot import InverseKinematicsRequest -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition, Robot, Settings +from arcor2_object_types.abstract import Robot, Settings +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_web import rest @@ -30,7 +31,7 @@ class PickUpObjectByPosition(JsonSchemaMixin): position: Position radius: float effector_type: EffectorType = EffectorType.SUCK - grasp_positions: list[GraspPosition] = field(default_factory=lambda: [GraspPosition.ALL]) + grasp_position: GraspPosition = GraspPosition.ALL object_type_name: object_type.Model3dType | None = None velocity: float = 50.0 payload: float = 0.0 @@ -41,7 +42,7 @@ class PickUpObjectByPosition(JsonSchemaMixin): class PickUpObjectById(JsonSchemaMixin): object_id: str effector_type: EffectorType = EffectorType.SUCK - grasp_positions: list[GraspPosition] = field(default_factory=lambda: [GraspPosition.ALL]) + grasp_position: GraspPosition = GraspPosition.ALL velocity: float = 50.0 payload: float = 0.0 safe: bool = True @@ -163,7 +164,7 @@ def pick_up_object_by_position( position: Position, radius: float, effector_type: EffectorType = EffectorType.SUCK, - grasp_positions: list[GraspPosition] | None = None, + grasp_position: GraspPosition = GraspPosition.ALL, object_type_name: object_type.Model3dType | None = None, speed: float = 50.0, safe: bool = True, @@ -176,7 +177,7 @@ def pick_up_object_by_position( :param position: Center of the area where the object should be found. :param radius: Search radius. :param effector_type: Type of end effector used for grasping. - :param grasp_positions: Preferred grasp positions. + :param grasp_position: Preferred grasp position. :param object_type_name: Optional object model type filter. :param speed: Relative speed. :param safe: Avoid collisions. @@ -188,9 +189,6 @@ def pick_up_object_by_position( assert 0.0 <= speed <= 100.0 assert 0.0 <= payload <= 5.0 - if grasp_positions is None: - grasp_positions = [GraspPosition.ALL] - with self._move_lock: rest.call( rest.Method.PUT, @@ -199,7 +197,7 @@ def pick_up_object_by_position( position=position, radius=radius, effector_type=effector_type, - grasp_positions=grasp_positions, + grasp_position=grasp_position, object_type_name=object_type_name, velocity=speed, payload=payload, @@ -212,7 +210,7 @@ def pick_up_object_by_id( self, object_id: str, effector_type: EffectorType = EffectorType.SUCK, - grasp_positions: list[GraspPosition] | None = None, + grasp_position: GraspPosition = GraspPosition.ALL, speed: float = 50.0, safe: bool = True, payload: float = 0.0, @@ -223,7 +221,7 @@ def pick_up_object_by_id( :param object_id: Collision/graspable object ID. :param effector_type: Type of end effector used for grasping. - :param grasp_positions: Preferred grasp positions. + :param grasp_position: Preferred grasp position. :param speed: Relative speed. :param safe: Avoid collisions. :param payload: Object weight. @@ -233,9 +231,6 @@ def pick_up_object_by_id( assert 0.0 <= speed <= 100.0 assert 0.0 <= payload <= 5.0 - if grasp_positions is None: - grasp_positions = [GraspPosition.ALL] - with self._move_lock: rest.call( rest.Method.PUT, @@ -243,7 +238,7 @@ def pick_up_object_by_id( body=PickUpObjectById( object_id=object_id, effector_type=effector_type, - grasp_positions=grasp_positions, + grasp_position=grasp_position, velocity=speed, payload=payload, safe=safe, diff --git a/src/python/arcor2_ur/scripts/grasp_planner.py b/src/python/arcor2_ur/scripts/grasp_planner.py index 1ec9a260..0e7ebde1 100644 --- a/src/python/arcor2_ur/scripts/grasp_planner.py +++ b/src/python/arcor2_ur/scripts/grasp_planner.py @@ -6,8 +6,7 @@ from arcor2.data import object_type from arcor2.data.common import Orientation, Pose, Position -from arcor2_object_types.abstract import EffectorType, GraspPosition -from arcor2_ur.common import CollisionSceneObject +from arcor2_ur.common import CollisionSceneObject, EffectorType, GraspPosition PRE_GRASP_OFFSET = 0.12 GRASP_OFFSET = 0.06 @@ -16,13 +15,13 @@ def generate_grasp_poses( obj: CollisionSceneObject, effector_type: EffectorType, - grasp_positions: list[GraspPosition] | None, + grasp_position: GraspPosition = GraspPosition.ALL, ros_mesh: RosMesh | None = None, ) -> list[tuple[Pose, Pose]]: mesh = object_to_mesh(obj, ros_mesh) - geometry = filter_geometry_by_grasp_position(mesh, grasp_positions) + geometry = filter_geometry_by_grasp_position(mesh, grasp_position) candidates = create_grasp_candidates_from_surfaces(geometry, effector_type) @@ -90,11 +89,11 @@ def quaternion_to_rotation_matrix(q: Orientation) -> np.ndarray: def filter_geometry_by_grasp_position( mesh: o3d.geometry.TriangleMesh, - grasp_positions: list[GraspPosition] | None, + grasp_position: GraspPosition = GraspPosition.ALL, ) -> o3d.geometry.TriangleMesh: """TOP/BOTTOM/LEFT/RIGHT/FRONT/BACK filter.""" - if grasp_positions is None or GraspPosition.ALL in grasp_positions: + if grasp_position == GraspPosition.ALL: return mesh mesh.compute_triangle_normals() @@ -113,13 +112,9 @@ def filter_geometry_by_grasp_position( kept = [] for triangle, normal in zip(triangles, normals): - for pos in grasp_positions: - direction = wanted_dirs.get(pos) - if ( - direction is not None and dot(normal, direction) >= 0.707 - ): # maximum allowed rotation from the original normal: 45° - kept.append(triangle) - break + direction = wanted_dirs[grasp_position] + if dot(normal, direction) >= 0.707: # maximum allowed rotation from the original normal: 45° + kept.append(triangle) filtered = o3d.geometry.TriangleMesh() filtered.vertices = mesh.vertices diff --git a/src/python/arcor2_ur/scripts/ros_worker.py b/src/python/arcor2_ur/scripts/ros_worker.py index c180efa3..5f6e53d9 100644 --- a/src/python/arcor2_ur/scripts/ros_worker.py +++ b/src/python/arcor2_ur/scripts/ros_worker.py @@ -13,13 +13,11 @@ from geometry_msgs.msg import Point # pants: no-infer-dep from geometry_msgs.msg import Pose as RosPose # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep -from moveit_msgs.msg import ( # pants: no-infer-dep - AttachedCollisionObject, - CollisionObject, - Constraints, - OrientationConstraint, - PositionConstraint, -) +from moveit_msgs.msg import AttachedCollisionObject # pants: no-infer-dep +from moveit_msgs.msg import CollisionObject # pants: no-infer-dep +from moveit_msgs.msg import Constraints # pants: no-infer-dep +from moveit_msgs.msg import OrientationConstraint # pants: no-infer-dep +from moveit_msgs.msg import PositionConstraint # pants: no-infer-dep from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep @@ -39,10 +37,9 @@ from arcor2.data.common import Joint, Orientation, Pose, Position from arcor2.data.robot import InverseKinematicsRequest from arcor2.logging import get_logger -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_storage import client as storage_client from arcor2_ur import topics -from arcor2_ur.common import CollisionSceneObject +from arcor2_ur.common import CollisionSceneObject, EffectorType, GraspableState, GraspPosition from arcor2_ur.exceptions import UrGeneral from arcor2_ur.object_types.ur5e import Vacuum from arcor2_ur.scripts.grasp_planner import generate_grasp_poses @@ -67,15 +64,6 @@ def pose_to_ros_pose(ps: Pose) -> RosPose: return rp -def mesh_scale_from_metadata(metadata: dict[str, Any]) -> tuple[float, float, float]: - scale = metadata.get("mesh_scale", (1.0, 1.0, 1.0)) - - if not isinstance(scale, (list, tuple)) or len(scale) != 3: - raise UrGeneral("Invalid mesh scale metadata.") - - return float(scale[0]), float(scale[1]), float(scale[2]) - - def mesh_file_to_ros_mesh( mesh_data: bytes, asset_id: str, @@ -206,7 +194,6 @@ def rotate_vector(q: Orientation, v: tuple[float, float, float]) -> tuple[float, vx, vy, vz = v # quaternion * vector * quaternion_conjugate - # vector berieme ako quaternion (vx, vy, vz, 0) tx = 2 * (y * vz - z * vy) ty = 2 * (z * vx - x * vz) tz = 2 * (x * vy - y * vx) @@ -833,7 +820,7 @@ def create_collision_object( elif isinstance(obj.model, object_type.Mesh): asset_id = obj.model.asset_id - scale = mesh_scale_from_metadata(obj.metadata) + scale = obj.metadata.get("mesh_scale", (1.0, 1.0, 1.0)) cache_key = (asset_id, scale) cached_mesh = self.mesh_models.get(cache_key) @@ -944,7 +931,7 @@ def attach_object( self, object_id: str, effector_type: EffectorType, - grasp_positions: list[GraspPosition] | None, + grasp_position: GraspPosition, velocity: float, payload: float, safe: bool, @@ -955,7 +942,7 @@ def attach_object( ros_mesh = None if isinstance(object.model, object_type.Mesh): asset_id = object.model.asset_id - scale = mesh_scale_from_metadata(object.metadata) + scale = object.metadata.get("mesh_scale", (1.0, 1.0, 1.0)) cache_key = (asset_id, scale) ros_mesh = self.mesh_models.get(cache_key) @@ -964,7 +951,7 @@ def attach_object( ros_mesh = mesh_file_to_ros_mesh(mesh_data, asset_id, scale) self.mesh_models[cache_key] = ros_mesh - grasp_options = generate_grasp_poses(object, effector_type, grasp_positions, ros_mesh) + grasp_options = generate_grasp_poses(object, effector_type, grasp_position, ros_mesh) logger.info(f"Generated {len(grasp_options)} grasp options for object {object_id}.") trail = 0 @@ -989,7 +976,7 @@ def attach_object( try: self.move_to_pose(pre_grasp_pose, velocity, payload, safe) - if self.tool: + if self.tool and effector_type == EffectorType.SUCK: self.suck(60) time.sleep(1.0) @@ -998,7 +985,7 @@ def attach_object( self.move_to_pose(grasp_pose, velocity, payload, safe) - if self.tool: + if self.tool and effector_type == EffectorType.SUCK: vac = Vacuum.from_dict(self.vacuum()) if vac.avg() < 20: self.release() @@ -1059,12 +1046,11 @@ def detach_object( self.release() time.sleep(1.0) - object.metadata.pop("attached_pose", None) - object.pose = target_pose - self.update_collisions(self.collision_objects) - self.move_to_pose(post_detach_pose, velocity, payload, safe) + self.remove_attached_object(object_id) + object.metadata.pop("attached_pose", None) + object.pose = target_pose object.metadata["state"] = GraspableState.WORLD self.update_collisions(self.collision_objects) @@ -1174,7 +1160,7 @@ def ros_worker_main( result = runtime.attach_object( kwargs["object_id"], EffectorType(kwargs["effector_type"]), - [GraspPosition(gp) for gp in kwargs["grasp_positions"]], + GraspPosition(kwargs["grasp_position"]), kwargs["velocity"], kwargs["payload"], kwargs["safe"], diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index 187dc197..4af9ab26 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -21,9 +21,8 @@ from arcor2.data.robot import InverseKinematicsRequest from arcor2.helpers import port_from_url from arcor2.logging import get_logger -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_ur import version -from arcor2_ur.common import CollisionSceneObject, parse_collision_body +from arcor2_ur.common import CollisionSceneObject, EffectorType, GraspableState, GraspPosition, parse_collision_body from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum from arcor2_ur.scripts.ros_worker import RosWorkerClient @@ -141,7 +140,7 @@ def put_start() -> RespT: content: application/json: schema: - $ref: '#/components/schemas/Pose' + $ref: Pose responses: 204: description: Ok @@ -276,18 +275,17 @@ def pick_up_object_by_position() -> RespT: enum: - SUCK default: SUCK - grasp_positions: - type: array - items: - type: string - enum: - - TOP - - RIGHT - - LEFT - - FRONT - - BACK - - BOTTOM - - ALL + grasp_position: + type: string + enum: + - TOP + - RIGHT + - LEFT + - FRONT + - BACK + - BOTTOM + - ALL + default: ALL object_type_name: type: string description: Optional model type filter. @@ -327,18 +325,7 @@ def pick_up_object_by_position() -> RespT: raise UrGeneral("Radius has to be >= 0.") effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK)) - grasp_positions = body.get( - "grasp_positions", - [ - GraspPosition.TOP, - GraspPosition.RIGHT, - GraspPosition.LEFT, - GraspPosition.FRONT, - GraspPosition.BACK, - GraspPosition.BOTTOM, - GraspPosition.ALL, - ], - ) + grasp_position = GraspPosition(body.get("grasp_position", GraspPosition.ALL)) object_type_name = ( object_type.MODEL_MAPPING[object_type.Model3dType(body["object_type_name"])] @@ -385,7 +372,7 @@ def pick_up_object_by_position() -> RespT: "pick_up_object", object_id=nearest_id, effector_type=effector_type, - grasp_positions=grasp_positions, + grasp_position=grasp_position, velocity=velocity, payload=payload, safe=safe, @@ -426,18 +413,17 @@ def pick_up_object_by_id() -> RespT: enum: - SUCK default: SUCK - grasp_positions: - type: array - items: - type: string - enum: - - TOP - - RIGHT - - LEFT - - FRONT - - BACK - - BOTTOM - - ALL + grasp_position: + type: string + enum: + - TOP + - RIGHT + - LEFT + - FRONT + - BACK + - BOTTOM + - ALL + default: ALL velocity: type: number format: float @@ -463,18 +449,7 @@ def pick_up_object_by_id() -> RespT: object_id = body["object_id"] effector_type = EffectorType(body.get("effector_type", EffectorType.SUCK)) - grasp_positions = body.get( - "grasp_positions", - [ - GraspPosition.TOP, - GraspPosition.RIGHT, - GraspPosition.LEFT, - GraspPosition.FRONT, - GraspPosition.BACK, - GraspPosition.BOTTOM, - GraspPosition.ALL, - ], - ) + grasp_position = GraspPosition(body.get("grasp_position", GraspPosition.ALL)) velocity = float(body.get("velocity", 50.0)) / 100.0 payload = float(body.get("payload", 0.0)) safe = bool(body.get("safe", True)) @@ -501,7 +476,7 @@ def pick_up_object_by_id() -> RespT: "pick_up_object", object_id=object_id, effector_type=effector_type, - grasp_positions=grasp_positions, + grasp_position=grasp_position, velocity=velocity, payload=payload, safe=safe, @@ -545,7 +520,7 @@ def place_object() -> RespT: - SUCK default: SUCK pose: - $ref: '#/components/schemas/Pose' + $ref: Pose velocity: type: number format: float @@ -632,7 +607,7 @@ def put_box() -> RespT: - pose properties: pose: - $ref: '#/components/schemas/Pose' + $ref: Pose metadata: type: object description: Optional metadata for graspable object support. @@ -712,7 +687,7 @@ def put_sphere() -> RespT: - pose properties: pose: - $ref: '#/components/schemas/Pose' + $ref: Pose metadata: type: object description: Optional metadata for graspable object support. @@ -797,7 +772,7 @@ def put_cylinder() -> RespT: - pose properties: pose: - $ref: '#/components/schemas/Pose' + $ref: Pose metadata: type: object description: Optional metadata for graspable object support. @@ -894,7 +869,7 @@ def put_mesh() -> RespT: - pose properties: pose: - $ref: '#/components/schemas/Pose' + $ref: Pose metadata: type: object description: Optional metadata for graspable object support. @@ -1035,7 +1010,7 @@ def get_joints() -> RespT: schema: type: array items: - $ref: '#/components/schemas/Joint' + $ref: Joint 500: description: "Error types: **General**, **StartError**." content: @@ -1060,7 +1035,7 @@ def put_ik() -> RespT: content: application/json: schema: - $ref: '#/components/schemas/InverseKinematicsRequest' + $ref: InverseKinematicsRequest responses: 200: description: Ok @@ -1069,7 +1044,7 @@ def put_ik() -> RespT: schema: type: array items: - $ref: '#/components/schemas/Joint' + $ref: Joint 500: description: "Error types: **General**, **DobotGeneral**, **StartError**." content: @@ -1162,7 +1137,7 @@ def get_eef_pose() -> RespT: content: application/json: schema: - $ref: '#/components/schemas/Pose' + $ref: Pose 500: description: "Error types: **General**, **StartError**." content: @@ -1210,7 +1185,7 @@ def put_eef_pose() -> RespT: content: application/json: schema: - $ref: '#/components/schemas/Pose' + $ref: Pose responses: 200: description: Ok @@ -1292,7 +1267,7 @@ def get_vacuum() -> RespT: content: application/json: schema: - $ref: '#/components/schemas/Vacuum' + $ref: Vacuum 500: description: "Error types: **General**, **StartError**." content: diff --git a/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py index e47573f4..8c4b74a8 100644 --- a/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py +++ b/src/python/arcor2_ur/tests/test_collision/test_obstacle_graspable.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box, Cylinder, Sphere -from arcor2_object_types.abstract import GraspableState from arcor2_scene_data import scene_service +from arcor2_ur.common import GraspableState from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls diff --git a/src/python/arcor2_ur/tests/test_interaction.py b/src/python/arcor2_ur/tests/test_interaction.py index 4106d635..9aa20d63 100644 --- a/src/python/arcor2_ur/tests/test_interaction.py +++ b/src/python/arcor2_ur/tests/test_interaction.py @@ -25,7 +25,7 @@ def test_basics(start_processes: Urls) -> None: pos.position.z -= 0.05 ot.move_to_pose("", pos, 0.5) pos_after = ot.get_end_effector_pose("") - assert orig_z - pos_after.position.z > 0.045 + assert orig_z - pos_after.position.z > 0.035 ot.suck() ot.release() diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py index 828c94e5..12358dc9 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py +++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_overhead_obstacle.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_side_obstacle(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED box = Box("Box2", 0.2, 0.2, 0.2) diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py index 3a3551e2..eb2ce96e 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py +++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_side_obstacle.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_side_obstacle(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED box = Box("Box2", 0.2, 0.2, 0.95) diff --git a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py index d06664e7..f34a574a 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py +++ b/src/python/arcor2_ur/tests/test_pick_and_move_whit_collisions/test_pick_and_move_wall.py @@ -5,8 +5,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box from arcor2.exceptions import Arcor2Exception -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -27,7 +27,7 @@ def test_pick_and_move_wall(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED box = Box("Wall1", 0.1, 2.0, 1.0) diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py index 2883f57e..607a6980 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_id.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py index d86e90d7..0a2aeb36 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_by_position.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_box_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py index f2cb2108..04f75d1e 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_box_left.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.LEFT]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.LEFT) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py index 2698ab45..30fb18c0 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_id.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_cylinder_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py index d9f870c4..5f8d9b58 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_cylinder_by_position.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_cylinder_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py index 62992462..b740af2b 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_id.py @@ -3,9 +3,9 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service from arcor2_storage import client as storage_client +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -39,7 +39,7 @@ def test_pick_and_place_mesh_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py index cafa1a99..3ae304f0 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_mesh_by_position.py @@ -3,9 +3,9 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service from arcor2_storage import client as storage_client +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -39,7 +39,7 @@ def test_pick_and_place_mesh_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py index afd48c55..52540ff8 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_rotated_box.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0.5, 0.5, 0.5, 0)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py index 4f1a1034..258c8ced 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_id.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_sphere_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py index 2aef9582..94bdad54 100644 --- a/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_and_place/test_pick_and_place_sphere_by_position.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_and_place_sphere_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py index 7c3b9557..467d461f 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_all.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.ALL]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.ALL) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py index a98ebc81..77027705 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_back.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.BACK]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.BACK) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py index c6776b61..ebc94b4e 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_bottom.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.BOTTOM]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.BOTTOM) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py index e1d502fa..c4be2b90 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_id.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py index ebc4c363..66e210f4 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_by_position.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py index 0e4249c6..c6d87dab 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_front.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(Y, X, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.FRONT]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.FRONT) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py index f3f604b4..91f75875 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_left.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.LEFT]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.LEFT) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py index 792480ec..d7697854 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_box_right.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Box -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_box_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.RIGHT]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.RIGHT) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py index 5e5e6b30..2a552ec4 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_id.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_cylinder_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py index b50e9786..06f6f2d9 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_cylinder_by_position.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Cylinder -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_cylinder_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py index eaf7df15..4e7ae0ec 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_id.py @@ -3,9 +3,9 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service from arcor2_storage import client as storage_client +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -39,7 +39,7 @@ def test_pick_up_mesh_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py index ae2eaa6f..1bddebe7 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_mesh_by_position.py @@ -3,9 +3,9 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Mesh -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service from arcor2_storage import client as storage_client +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -39,7 +39,7 @@ def test_pick_up_mesh_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py index 48f28d7e..31c550a5 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_id.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_sphere_by_id(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_id(object.id, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_id(object.id, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py index b69245a6..9219042d 100644 --- a/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py +++ b/src/python/arcor2_ur/tests/test_pick_up/test_pick_up_sphere_by_position.py @@ -2,8 +2,8 @@ from arcor2.data.common import Orientation, Pose, Position from arcor2.data.object_type import Sphere -from arcor2_object_types.abstract import EffectorType, GraspableState, GraspPosition from arcor2_scene_data import scene_service +from arcor2_ur.common import EffectorType, GraspableState, GraspPosition from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @@ -24,7 +24,7 @@ def test_pick_up_sphere_by_position(start_processes: Urls) -> None: scene_service.upsert_graspable(object, Pose(Position(X, Y, Z), Orientation(0, 0, 0, 1)), GraspableState.WORLD) time.sleep(1) - ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, [GraspPosition.TOP]) + ot.pick_up_object_by_position(Position(X, Y, Z), 0.5, EffectorType.SUCK, GraspPosition.TOP) assert ot.graspable_state(object.id) == GraspableState.ATTACHED diff --git a/src/python/arcor2_ur/tests/test_suck_effector.py b/src/python/arcor2_ur/tests/test_suck_effector.py index 89360fba..d96fdc52 100644 --- a/src/python/arcor2_ur/tests/test_suck_effector.py +++ b/src/python/arcor2_ur/tests/test_suck_effector.py @@ -28,6 +28,6 @@ def test_suck_effector(start_processes: Urls) -> None: pos.position.z -= 0.05 ot.move_to_pose("", pos, 0.5) pos_after = ot.get_end_effector_pose("") - assert orig_z - pos_after.position.z > 0.045 + assert orig_z - pos_after.position.z > 0.035 ot.cleanup()